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ABSTRACT 


Two methods of estimating the attitude position of a spacecraft are examined in this 
thesis: the extended Kalman filter (EKF) and the unscented Kalman filter (UKF). In 
particular, the UnScented QUaternion Estimator (EISQUE) derived from [4] is 
implemented into a spacecraft model. Eor generalizations about the each of the filters, a 
simple problem is initially solved. These solutions display typical characteristics of each 
filter type. The LIKE is very attractive in spacecraft attitude estimation, given that 
spacecraft dynamics are highly nonlinear. Eor nonlinear systems, the UKF is of 
particular interest because it uses a carefully selected set of sample points that more 
accurately map the probability distribution than the linearization of the standard extended 
Kalman filter. This leads to faster convergence of the attitude solution from largely 
inaccurate initial conditions. The filter created in this thesis is formulated based on 
Markley and Crassidis’s work on standard attitude-vector measurements using a gyro- 
based model for attitude propagation. From the standard attitude vector measurements, 
the global attitude parameterization is found and given by a quaternion, while a 
generalized three-dimensional attitude representation is used to define the local attitude 
error. The multiplicative quaternion-error is then found from the local error. The 
simulation results indicate that the unscented filter is more robust than the extended 
Kalman filter. 


V 



THIS PAGE INTENTIONALLY LEET BLANK 


VI 



TABLE OF CONTENTS 


L INTRODUCTION.1 

A. BACKGROUND.1 

B. SHORT OVERVIEW OF ATTITUDE ESTIMATION.2 

C. RECENT CUBESAT ADCS SYSTEMS.4 

1. Canadian Advanced Nanospace experiment (CanX) ADS.4 

2. AISSat-lADS.6 

3. Radio Aurora Explorer (RAX) ADS.7 

II. GENERAL EXTENDED AND UNSCENTED KALMAN FILTERING 

METHODS FOR THE ESTIMATION OF DYNAMIC SYSTEMS.9 

A. BACKGROUND.9 

B. CONTINUOUS-TIME EXTENDED KALMAN FILTER.12 

C. DISCRETE-TIME LINEAR AND EXTENDED KALMAN FILTERS ..15 

D. UNSCENTED KALMAN FILTER.18 

E. IMPLEMENTATION OF EKE AND UKF METHODS USING THE 

SIMPLE PENDULUM PROBLEM.25 

F. EKE AND UKF ESTIMATION RESULTS USING THE SIMPLE 

PENDULUM PROBLEM.28 

III. IMPLEMENTATION OF EKE AND UKF FOR SPACECRAFT 

ATTITUDE DETERMINATION.37 

A. GENERALIZATIONS.37 

B. ANALYTICAL MODELING AND SETUP FOR ATTITUDE 

DETERMINATION SIMULATIONS.37 

1. Background.37 

2. Dynamics and Kinematics.38 

3. Spacecraft Attitude Disturbance Torques.39 

4. Spacecraft Sensor and Noise Modeling.40 

C. ATTITUDE KINEMATICS FOR QUATERNION ESTIMATION.41 

D. CRASSIDIS AND MARKLEY’S UNSCENTED QUATERNION 

ESTIMATOR (USQUE).45 

1. Initialization.48 

2. Calculation of Sigma Points.48 

3. Covariance and Gain Calculations.51 

4. Update Routine for States and Error Covariance.52 

E. IMPLEMENTATION OF THE EXTENDED KALMAN FILTER.53 

IV. COMPARISON OF SIMULATION RESULTS USING EKE AND UKF 

FILTERING METHODS.57 

A. SIMULATION CONDITIONS.57 

B. SIMULATION 1 RESULTS.57 

C. SIMULATION 2 RESULTS.63 

D. DISCUSSION OF RESULTS AGAINST PREVIOUS LITERATURE ...69 

vii 


































V. CONCLUSION.71 

A, SUMMARY.71 

B. FUTURE WORK.71 

APPENDIX A - SIMPLE PENDULUM SIMULATION.73 

APPENDIX B - SPACECRAFT ATTITUDE DETERMINATION SIMULATION.81 

LIST OF REFERENCES.115 

INITIAL DISTRIBUTION LIST.117 


viii 










LIST OF FIGURES 


Figure 1. Dr. William Pickering, Dr. James Van Allen, and Dr. Wernher Von Braun 

hold a model of the Explorer 1 vehicle above their heads. Credit: NASA.1 

Figure 2. CanX-1 Agilent Technologies CMOS Imager.5 

Figure 3. Computer Rendering of CanX-2.5 

Figure 4. Computer Rendering of AISSat-1, from [7].6 

Figure 5. Illustration of Extended Kalman filter linearization of nonlinear function 

and the related Gaussian distribution.13 

Figure 6. Illustration of the unscented Kalman filter sigma-points propagation.22 

Figure 7. Simple Pendulum Problem.26 

Figure 8. Simulink Block Diagram of Simple Pendulum Model.27 

Figure 9. Angular Errors in EKF with 3a Error Bounds Simulation 1 (large 0).29 

Figure 10. Angular Errors in UKF with 3a Error Bounds Simulation 1 (large 0).30 

Figure 11. Angular Rate Errors in EKF with 3a Error Bounds Simulation 1 (large 0) ....30 

Figure 12. Angular Rate Errors in UKF with 3a Error Bounds Simulation 1 (large 0)....31 

Figure 13. Moment of Inertia Errors in EKF with 3a Error Bounds Simulation 1 

(large 0).32 

Figure 14. Moment of Inertia Errors in UKF with 3a Error Bounds Simulation 1 

(large 0).32 

Figure 15. Angular Errors in EKF with 3a Error Bounds for Simulation 2 (small 0).33 

Figure 16. Angular Errors in UKF with 3a Error Bounds for Simulation 2 (small 0).34 

Figure 17. Angular Rate Errors in EKF with 3a Error Bounds for Simulation 2 

(small 0).34 

Figure 18. Angular Rate Errors in UKF with 3a Error Bounds for Simulation 2 

(small 0).35 

Figure 19. Moment of Inertia Errors in EKF with 3a Error Bounds for Simulation 2 

(small 0).35 

Figure 20. Moment of Inertia Errors in UKF with 3a Error Bounds for Simulation 2 

(small 0).36 

Figure 21. Attitude Dynamics and Kinematics Simulink Block.38 

Figure 22. Unscented Quaternion Estimator Flow Chart.46 

Figure 23. Unscented Kalman Filter Block - Fevel 1.47 

Figure 24. Unscented Kalman Filter Block - Fevel 2.48 

Figure 25. Extended Kalman Filter Flow Chart.54 

Figure 26. Simulation 1 Quaternion Attitude Error with 3a Bounds for EKF.58 

Figure 27. Simulation 1 Generalized Rodriguez Parameter Attitude Error with for 3a 

Bounds for UKF.59 

Figure 28. Simulation 1 EKF Bias Errors with 3a Bounds.60 

Figure 29. Simulation 1 UKF Bias Errors with 3a Bounds.61 

Figure 30. Comparison of EKF and UKF Normalized Attitude Errors for 

Simulation 1.62 

Figure 31. Comparison of EKF and UKF Normalized Bias Errors for Simulation 1.63 

Figure 32. Simulation 2 Quaternion Attitude Error with 3a Bounds for EKF.64 


IX 

































Figure 33. Simulation 2 Generalized Rodriguez Parameter Attitude Error with for 3a 

Bounds for UKF.65 

Figure 34. Simulation 2 EKE Bias Errors with 3a Bounds.66 

Figure 35. Simulation 2 LIKE Bias Errors with 3a Bounds.67 

Figure 36. Comparison of EKE and UKF Normalized Attitude Errors for 

Simulation 2.68 

Figure 37. Comparison of EKE and UKF Normalized Bias Errors for Simulation 2.69 

Figure 38. Norm of Attitude Errors, from [4].70 


X 









LIST OF TABLES 


Table 1. Standard Symbols of Kalman Filtering, from [10].10 

Table 2. Speeial State Spaee Notation, from [10].11 

Table 3. Continuous-Time Extended Kalman Filter, from [9].15 

Table 4. Diserete-Time Linear Kalman Filter, from [9].18 

Table 5. Unseented Kalman Filter, from [9].25 

Table 6. Summary of Gyro Noise Coeffieients, from [4].40 

Table 7. Deseription of Inputs and Outputs for UKF Bloek - Level 1.47 

Table 8. Summary of EKF Equations, from [9].55 











THIS PAGE INTENTIONALLY LEET BLANK 



LIST OF ACRONYMS AND ABBREVIATIONS 


1-U Refers the 1-CubeSat standard of 10 em x 10 em x 10 em 

3-U Refers to the 3-CubeSat standard of 10 cm x 10 cm x 30 cm 

ADCS Attitude Determination and Control System 

ADS Attitude Determination System 

CanX The Canadian Advanced Nanospace experiment 

EKF Extended Kalman Eilter 

GPS Global Positioning System 

IMU Intertial Measurement Unit 

NACE Nanosatellite Advanced Concepts Eaboratory 

NASA National Aeronautics and Space Administration 

NPS The Naval Postgraduate School 

ORS Office of Responsive Space 

QUEST Quaternion Estimator 

RAX Radio Aurora Explorer 

UKE Unscented Kalman Eilter 

USQUE UnScented QUaternion Estimator 



THIS PAGE INTENTIONALLY LEET BLANK 


XIV 



ACKNOWLEDGMENTS 


The completion of this program would not have been possible without the help of 
others. I would like to acknowledge the National Aeronautics and Space Administration, 
particularly NASA Ames Research Center Director Dr. S. Pete Worden and Engineering 
Director Pete Klupar, for supporting me financially and ensuring that my education gets 
put to good use when I return. 

I would also like to thank the following individuals for their invaluable assistance 
in the completion of this thesis: 

Professor Dr. Marcello Romano for allowing me to be part of his research. 

Dr. Hyun-wook Woo, without your help and support I would have progressed 
endlessly without direction. Your help was greatly appreciated. 

Ms. Karen Andersen for helping me find this wonderful opportunity of a free 
education and navigating the possibly endless road of government politics. This 
simply would not have been possible without your dedication and perseverance. 

Ms. Trang Luong for her support and patience during these two years while 
commuting between San Jose and Monterey, always reminding me what’s most 
important in life, each other. The future always seems brighter when you can see 
yourself in it with someone special. 

My classmates. Space Systems Engineering and Operations Students 2010, for 
allowing me to be a part of the military community for these short years. The long 
nights of studying are not so terrible when you are in the company of heroes. A bit 
cliche, but true. I hope all the best for your future endeavors. 

Eastly, to Avilio Pena and Juan Garces Diaz who both passed away during my time 
here, for teaching me the meaning of service and the importance of education at an 
early age. 


XV 



THIS PAGE INTENTIONALLY LEET BLANK 


XVI 



I. INTRODUCTION 


A, BACKGROUND 

In August of 1960, the United States Air Foree and the Central Intelligenee 
Ageney sueeessfully launehed the world’s first reeonnaissance satellite, Corona. The 
imaging resolution was 8 meters and taken on film. The program lasted for 12 years, and 
ushered in the era of space-based reconnaissance and intelligence gathering that would be 
iconic of the Cold War. Since the beginning of spacecraft building, organizations have 
prized themselves on pushing the envelopes of technology. Programs such as NASA’s 
Explorer, TIROS, and Pioneer later proved that the U.S. was investing heavily on space 
technologies. 
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Figure 1. Dr. William Pickering, Dr. James Van Allen, and Dr. Wernher Von Braun 
hold a model of the Explorer 1 vehicle above their heads. Credit: NASA 


Historically, the building of spacecraft has been a lengthy process, often taking 
many years or even decades. Recently, however, a new methodology for building 
spacecraft has transpired. Organizations such as The Office of Responsive Space (ORS) 
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have been ereated to change this expensive and lengthy process into one that focuses on 
providing a "good enough" service in a timely manner [1], This push for faster programs 
has also led areas of the industry to build smaller systems in attempts to utilize the 
leftover-over payload mass of launch vehicles. This is more commonly referred to as a 
"secondary payload." The industry push combined with the emerging university 
nanosatellite community has created an influx of new commercialism for space-based 
hardware. 

One of the limiting technologies in the small spacecraft arena is attitude 
determination and control systems (ADCS). While currently there is an increased interest 
in this area, a limited number of complete solutions in a 3U or lU-class nanosatellite 
have been demonstrated on-orbit. Many proposed solutions are also not affordable to this 
community. While companies like Boeing, Honeywell, and Sinclair are working on 
hardware solutions, the problem of attitude determination and control can be attacked 
from both sides. That is to say, as the hardware is being developed, both academic and 
commercial institutions can focus their resources on the optimal estimation and control 
theory problems. The lack of an affordable hardware should not inhibit willing parties to 
develop solutions and methods for the small spacecraft ADCS problem. 

B, SHORT OVERVIEW OF ATTITUDE ESTIMATION 

One of the most common estimation techniques that has been widely used for 
various dynamics systems is the Kalman filter. While the filter was initially designed for 
linear systems, variations of this filter have been developed in particular for nonlinear 
systems. The extended Kalman filter can be used on nonlinear systems and is based on 
linearizing the system dynamics. While this is potentially attractive for the nonlinear 
spacecraft attitude control problem there are several associated with the nonlinearization 
[ 2 ]. 

While the EKF has proven to be a popular tool for nonlinear estimation, it 
continues to endure some fundamental issues inherent in the linearization process, which 
can be the potential cause of divergence. A later development for nonlinear estimation 
was developed by Julier and Uhlmann and is called the “unscented” Kalman filter [3]. 
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The UKF is “founded on the intuition that it is easier to approximate a probability 
distribution than it is to approximate an arbitrary nonlinear function or transformation 
[3]." The UKF successfully avoids the EKF linearization step by introducing a set of 
sample points that capture the higher order statics of the system. Finally, the UKF 
method has been developed to estimate the quaternions associated with the attitude of a 
spacecraft [4], The numerical simulations presented in these studies have illustrated the 
superior performance of the UKF in this context. 

The primary goal of this thesis is to develop and verify estimation algorithms and 
simulation code for a spacecraft attitude determination system (ADS). In particular, the 
two estimation methods that are compared for determining the attitude are the extended 
Kalman filter (EKF) and the unscented Kalman filter (UKF). Each filter is evaluated 
based on error computation time. The inherent linearity and nonlinearity of each type of 
filter is examined by choosing related problems that highlight issues in trying to use a 
linear filter (EKE) to solve a nonlinear problem. To do this, two separate simulations 
codes were designed. These simulation codes include an accurate spacecraft model where 
torque disturbances. Earth physics, and orbital mechanics are accounted for, as well as 
sensor models of an inertial measurement unit and magnetometer. 

A simplified problem was used to verify the behavior of both estimation methods 
on linear and nonlinear dynamics. Eor this, the simple pendulum was used as a way to 
show how each filter can be used to estimate the states of a given dynamic problem. 
After this problem was worked, these filters were used as analogs against a simulated 
spacecraft model. Primarily focusing on the UKF, this thesis discusses the differences 
between the two filters and focuses on the benefits of using nonlinear estimation. It is 
widely known that there are many benefits to nonlinear estimation. The UKF is very 
attractive in spacecraft attitude estimation, given that spacecraft dynamics are highly 
nonlinear. This thesis highlights these benefits while solving both the EKF and UKF 
spacecraft attitude estimation problem. While previous theses discussed the nuances of 
characterizing these types of sensors for inclusion in the simulation [4], this paper will 
focus on the estimation methods as they apply to attitude determination. 
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C. RECENT CUBESAT ADCS SYSTEMS 

Several spacecraft that have implemented ADCS systems into the small CubeSat 
standard. This section discusses three of these, which include the following: 

• Canadian Advanced Nanospace experiment (CanX) -The University of 
Toronto Institute for Aerospace Studies Space Flight Laboratory (UTIAS 
SLF) 

• AISSat-1 - The University of Toronto Institute for Aerospace Studies 
Space Flight Laboratory (UTIAS SLF) 

• Radio Aurora experiment (RAX) - The University of Michigan 

1. Canadian Advanced Nanospace experiment (CanX) ADS 

The Canadian Advanced Nanospace experiment (CanX) program is run by the 
University of Toronto Institute for Aerospace Studies (UTIAS) Space Flight Laboratory. 
CanX-1 launched on June 2003 from Plesetsk, Russia, and was a 1-U CubeSat that 
consisted of several ADS hardware components. The primary mission of CanX-1 was to 
demonstrate the several experimental ADS components. The CanX-1 ADS package 
consisted of a CMOS Imager for ground-controlled horizon sensing and star tracking, 
active three-axis magnetic stabilization and a Global Positioning System (GPS) receiver 
that was modified to work in low Earth orbit. Figure 2 shows a picture of the CanX 
CMOS imager used for star tracking [6]. 
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Figure 2. CanX-1 Agilent Technologies CMOS Imager 

CanX-2, which launched in April 2008, uses many of the same types of ADS 
systems. The CanX-2 ADS uses a suite of sun sensors and a three-axis magnetometer. 
Both CanX-1 and CanX-2 use a standard extended Kalman filter to estimate the attitude 
of the spacecraft. 



Figure 3. Computer Rendering of CanX-2 
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AISSat-1 ADS 


AISSat-1 is a 6-kg Norwegian nanosatellite, being constructed on behalf of 
government of Norway by UTIAS/SFL, whose primary mission is to investigate the 
feasibility and performance of a spacecraft-based Automatic Identification System (AIS) 
sensor in low-Earth orbit as a means of tracking maritime assets. AISSat-1 is intended as 
both a research and development platform, and a demonstration mission for a larger 
operational capability. 



Figure 4. Computer Rendering of AISSat-1, from [7] 


A full 3-axis attitude determination and control system provides attitude 
stabilization and fine pointing for AISSat-1. The satellite is able to point in either and 
inertial orientations, or an orbit-frame-fixed orientation, including on nadir. Attitude 
sensors consist of six sun sensors, a magnetometer and rate gyros. Three orthogonally 
mounted reaction wheels and three magnetorquer coils controls the actuation of the 
satellite. The magnetorquer is used for de-tumbling and momentum dumping while the 
reaction wheels provide fine attitude pointing capability. The attitude control system is 
able to maintain several degree level pointing accuracy and stability over the course of 
the entire orbit, including eclipse. For attitude estimation, this spacecraft also 
implemented an extended Kalman filter [7]. 
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3, Radio Aurora Explorer (RAX) ADS 

The Radio Aurora Explorer (RAX) spaeeeraft, currently being developed by The 
University of Michigan is a 3U CubeSat, which will also implement and attitude 
determination system. The primary scientific objective of the Radio Aurora Explorer 
(RAX) mission is to understand the microphysics of plasma instabilities that lead to field- 
aligned irregularities (EAI) of electron density in the polar lower (80-400 km) 
ionosphere. Eor attitude control, an inertial measurement unit in conjunction with sun 
sensors and magnetometers will observe the time it takes the passive magnetic attitude 
control system to de-tumble the spacecraft after deployment. This system will implement 
a continuous-discrete extended Kalman filter. They will implement a 13 state filter, 
which will consist of 3 position, 3 velocity, 4 quaternions, and 3 angular rates. The team 
will implement the QUaternion ESTimator (QUEST) method developed by Shuster and 
Oh [8]. Eiterature describes the QUEST method as computationally expensive; however, 
the information will be gathered on orbit and processed on the ground to eliminate 
computational constraints on the filtering process. Some of the ADS hardware will 
include six 3-axis magnetometers, nine sun sensors and an inertial measurement unit, 
which will consist of a 3-axis gyroscope. 
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II. GENERAL EXTENDED AND UNSCENTED KALMAN 
FILTERING METHODS FOR THE ESTIMATION OF 
DYNAMIC SYSTEMS 

A, BACKGROUND 

Accurate attitude knowledge is essential for many spaeecraft missions. Kalman 
filtering has been widely known since the 1960s as a method for filtering out noise in a 
given measurement. Theoretically, the Kalman filter is a sequential optimal estimator for 
what is called the linear-quadratic problem, which is the problem of estimating the 
instantaneous "state" of a linear dynamie system ineluding its uncertainty—by using 
measurements linearly related to the state corrupted by white noise [10]. For attitude 
determination, several types of Kalman filters have been developed over the years. This 
seetion describes two basic types of Kalman filters, the extended Kalman filter (EKF) and 
the unseented Kalman filter (UKF). There have been many technieal papers written on 
Kalman filtering for state estimation [2] [3]. This chapter will start the discussion with 
the continuous-time Kalman filter as a base line. Several textbooks use a variety of 
nomenclature to deseribe this estimation proeess. As a standard, the following tables 
define the notation used in this thesis. These tables are also consistent with [9] and [10]. 
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Table 1. 


Standard Symbols of Kalman Filtering, from [10] 


Symbols 

Symbol Definition 

F 

Dynamie eoeffieient matrix (state matrix) of a eontinuous linear 

differential equation defining a dynamie system 

G 

Coupling matrix between random proeess noise and the state of a dynamie 

system 

H 

Measurement sensitivity matrix defining the linear relationship between 

the state of the dynamie system and measurements that ean be made, (also 

known as a eoeffieient matrix [9]) 

K 

Kalman gain matrix 

P 

Covarianee matrix of state estimation uneertainty 

Q 

Covarianee matrix of proeess noise in the system state dynamies also 

ealled the proeess noise eovarianee 

R 

Covarianee matrix of observational (measurement) uncertainty also called 

the measurement noise covariance 

X 

State vector 

y 

Vector (or scalar) of measured values. 

o 

State transition matrix of a discrete dynamic system 


10 





Table 2. Special State Space Notation, from [10] 


Symbols 

Symbol Definition 


The i-th component of the vector x, or the i-th element of the sequence. The 
sub-index k refers to the sequence of propagation as it occurs in the filtering 
process, i.e. k+1 can be referred to as the “update” term that is determined 
from the same term calculated previously, (0 = (i) + noise 

X 

An estimate of the value of x. 

K 

A priori estimate of the x^, conditioned on all prior measurements except 

the one at time 

Xk 

A posteriori estimate of the x, conditioned on all available measurements at 
time 4 

y 

A measurement of some quantity we can estimate to the state vector from. 

X 

Derivative of x with respect to time 


The Kalman filter uses a parametric characterization of the probability 
distribution of its estimation errors in determining the optimal filtering gains, and it is the 
probability distribution that can be used for assessing its performance as a function of the 
“design parameters” of an estimation system [10]. Some of these can include: 

• the types of sensors used, 

• the locations and orientations of the various sensor types with respect to 
the system to be estimated, 

• the allowable noise characteristics of the sensors, 

• the data sampling rates for the various sensor types, and most importantly, 

• the level of model simplification to reduce implementation requirements. 
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B, CONTINUOUS-TIME EXTENDED KALMAN FILTER 

For nonlinear systems, sueh as spaeecraft dynamics, the extended Kalman filter 
(EKF) has been previously proposed in literature and used on-board many spacecraft [2], 
In the EKF, the state transition and observation models do not need to be linear functions 
representing the state, granted they are differentiable. Given that a vast majority of 
nonlinear problems can be described with differentiable nonlinear functions, the 
Continuous-Time EKF can often be used. The Continuous-Time EKF is very similar to 
the Continuous-Time Linear Kalman filter [9]. The derivation of the Continuous-Time 
EKE starts with the continuous non-linear system model below: 

x(f) = f(x(f),u(f),t) + G(f)w(f) 2.1 

y(f) = h(x(0,0 +v(r) 2.2 

where it is important to note that f(x(t), u(t),t) represents nonlinear continuous function or 
the state transition model, while G(t) and w(t) represent the coupling matrix and 
continuous-time covariance respectively. Eor Equation 2.2, f(t) represents the measured 
nonlinear observed model using a continuous function h(x(t),t) plus the continuous-time 
covariance, v(t). 

The inherent linearization process can cause the filter to diverge, as the Gaussian 
input does not necessarily produce a Gaussian output [9]. To continue, we must assume 
that, for our purposes, a linear representation of our non-linear system will suffice. Eor 
example, this method can certainly be used for functions where small angle 
approximation is valid. Examples of the limitations are discussed in detail in Section E. 
Eor the EKE, we must also assume that the true state of the system is sufficiently close to 
the estimated state. Therefore, the error dynamics can be reasonably approximated by a 
linearized first order Taylor series expansion. The first order expansion of f(x(f),u(f),f) 
about a nominal state \{t) becomes: 
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f u(t), t) = f (x(0, u(0,0 + 

ox 


[x(t)-x(t)] 


x(0 


2.3 


where x(f) is close to x(f). Similarly, the output in Equation 2.3 becomes [9]; 


h(x(0,0 = h(x(0,0 + 


5h 

dx 


[x(t)-x(t)] 


x(r) 


2.4 


Here the EKE solves this problem by calculating the Jacobians of / and h around the 
estimated state, which in turn yields a trajectory model function centered around this 
state. Eigure 5 shows this graphically [11]. 



Eigure 5. Illustration of Extended Kalman filter linearization of nonlinear function 

and the related Gaussian distribution. 

To find the estimate of the state, the extended Kalman filter continues with 
assumption made earlier, that x(f) = x(t). Thus, the expectation of both Equations 2.3 
and 2.4 gives the following equation, where E represents the conditional mean or 
expectation [9]. 
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£ {f (x(0, u(0,0} = f (x(0, u(0, t) 
£{h(x(0,0}=h(x(0,0 


2.5 


2.6 


Therefore, the extended Kalman filter for the state and output estimate is given by the 
following two equations [9]. 


x(0 = f(x(0,u(0,0 + ^(0[y(0-h(x(0,0' 


y(0 = h(x(0,0 


2.8 


Because the equation of the measurement of the state vector has the same structure as the 
linear Kalman filter, we can use the covariance expression shown in Table 3. The 
following table summarizes the equations for the continuous-time extended Kalman 
filter. 


14 



Table 3. 


Continuous-Time Extended Kalman Filter, from [9] 


Model 

x(0 = f (x(t), u(0, t) + G(0w(0, 
y(0 = h(x(0,0 + v(0 

Initialize 

x(c) = Xo 

^o=^{x(c)x^(^o)} 

Gain 


Covariance 

P{t) = F{^{t),t)P{t) + P{t)F^{^{t),t) 

^(x(o, 0^ ‘(o^(x(OT)no +G(0G(0G^(0 

17 /"' t ^ Tjf’' \ 

\{t) \{t) 

Estimate 

x(0 = f(x(0,u(0,0+K(0[y(0 -h(x(0,0] 


C. DISCRETE-TIME LINEAR AND EXTENDED KALMAN FILTERS 

While understanding the basies of the eontinuous-time extended Kalman filter is 
valuable in the sense that it ean often be used to solve entire solutions analytieally, 
implementation of this is not praotieal. In most cases, the control system is responding to 
different given inputs. The use of real-time processing is inevitable in the practical 
implementation of estimating dynamic systems. Thus, the continuous-time Kalman filter 
must be discretized so that it may be applied to iterative methods. This section describes 
how the Kalman filter is derived. 

Derivation of the discrete-time filter and the extended Kalman filter are very 
similar. To derive the discrete-time Kalman filter, an assumption must be made that both 
the model and measurement are available in discrete form. Here, we can start with the 
non-linear "truth" model shown below [9]: 






2.10 


where ® is the state transition matrix, T is the eontrol-input matrix that is applied to the 
eontrol vector uu, and ^ is the noise matrix. The definition of ® , T ^ and ^ are shown 
below. 

0 = 2.11 
(•At rp, 

r= f e^'dt 

_Jo 

(•At rr, 

Y = f dt 

Jo 

where B and G are the coefficient matrices taken from the continuous system. Again, in 
Equations 2.9 and 2.10 Wk(t) and Vk(t) are assumed to be zero-mean Gaussian white-noise 
processes and their covariance's are given by the expectation equations [9]; 





0 j 
Qk k = j 



0 j 
K k = j 


2.14 


2.15 


The Qk matrix accounts for the state process noise while the Rk matrix accounts 
for the expected measurement noise. These equations imply that the errors are not 
correlated forward or backward in time. We can also assume that Vk and Wk are 
uncorrelated so: 

^{vX} = 0 2.16 

Updating the current estimate of the state x^. to obtain based upon all k+l 

measurement subsets assumes that the gain (K) can vary in time. This propagation can 
be done using [9]: 
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2.17 


A — -y A _|_ I ^ 


Furthermore, the updated state is given by: 


[y^ - 


2.18 


where is the measurement veetor. The gain Kk changes with time properly weighting 
the relative confidence of the accuracy of the propagated state verses the measured state. 
To find Kk, first the state error and error covariance matrixes must be defined [9]: 


Pk 

where 




2.19 

2.20 


Substituting Equations 2.9 and 2.17 into Equation 2.20 and substituting the resulting 
equation into Equation 2.19 leads to: 


= + 2.21 

Because w^, and x^ are uncorrelated the terms e|w^x^^| =e|x^w[| = 0. To find the 

updated error covariance matrix, we can use Equations 2.10 and 2.18. Then substitution 
of the resulting equation into Equation 2.20 leads to: 


p:- 






2.22 


To find the gain K, the trace of error covariance matrix is minimized. Solving gives: 


K, = p;hI {K)[h,(x;)i^hI (x,:)+«„]■' 


2.23 
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As mentioned previously, the extended Kalman filter and diserete-time Kalman 
filter are nearly identieal. The only difference between these two are the initial model 
equations and the propagation equations. The extended Kalman filter assumes that the 
model is a continuous function and thus be differentiable. This is clearly evident in Table 
4. 

Table 4. Discrete-Time Linear Kalman Filter, from [9] 


Model 

X,., =‘I>A+r,u,+Yw„ w, ~Af(0,e,) 
yk=H,x, + y„ y,~N{0,R,) 

Initialize 

o 

<>< 

II 

<>< 



Gain 

K, = p^Hl (x-)+/?,]■' 

Update 



p:=[i-k,h,]p- 

Propagation 

xf+i + 




D, UNSCENTED KALMAN FILTER 

The inherent issue with propagating Gaussian random variables through a 
nonlinear function can also be approached using a technique described as the unscented 
transform. While the extended Kalman filter has many applications, and is the most 
popular method for nonlinear estimation to date, the unscented Kalman filter (UKF) was 
proposed by Julier, Uhlmann, and Durrant-Whyte [12] to overcome the instabilities 
associated with the EKF. While the EKF typically works well in the regions where the 
first-order Taylor series linearization adequately approximates the nonlinear probably 
distribution, a primary area of concern is during the initialization stage, where the 


18 




estimated initial state ean be far from the true state [9]. The UKF typieally involves more 
eomplex eomputations than the EKF, but has the following advantages: 

1. the expeeted error is lower than the EKF 

2. it ean be applied to non-differentiable funetion 

3. it avoids the derivation of Jaeobian matriees 

4. it is valid to higher-order expansions than the standard EKF [4] 

The UKF ean be thought of as an extension of the traditional Kalman filter for the 
estimation of nonlinear systems that implements the unseented transformation. The 
unseented transformation uses a set of sample, or sigma, points that are determined from 
the a priori mean and eovarianee of the state. The sigma points undergo the nonlinear 
transformation. Then the a posteriori mean and eovarianee of the state are determined 
from the transformed sigma points. This approaeh gives the UKF better eonvergenee 
eharaeteristies and greater aeeuraey than the EKF for nonlinear systems [13]. The ability 
of the UKF to aeeurately estimate nonlinearities make it attraetive for implementation on 
spaeeeraft as the state and observations are inherently nonlinear. This seetion deseribes 
the basie derivation of the unseented Kalman filter, while the subsequent seetions 
deseribe the implementation of the UKF for attitude determination. 

The derivation of the unseented Kalman filter starts by seleeting a nonlinear 
system defined by [9]: 

x,.i=f(x„^) + G,w, 2.24 

y, =h(x„k) + v, 2.25 

where Xk is the n x 1 state veetor and yk is the m x 1 measurement veetor. It is 
interesting to note that a eontinuous-time model ean also be expressed in the form of 
Equation 2.24. Similar to the previous derivations, Vk represents the measurement-error 
noise while Wk deseribes the white Gaussian proeess noise with eovarianees given by 

E |w(t)w^(r)| = Q{t)5{t - z) 2.26 
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£|w(Ov^(r)| = R{t)5{t -t) 
E |v(Ow^(r)| = 0 


2.27 


2.28 


The covariance matrices of each of these are given by Qk and Rk respectively [4]. The 
Kalman filter update equations are rewritten from Table 4 as [3]; 

K=K + f^k^k 2.29 

p: = p--K,prKi 2.30 


where is the innovations process, given by 

= yk-HK’^kk) 


2.31 


The covariance of the innovations process, is given by P/" [4]. 

PkZ = PkZ + Rk.. 2.32 

The Kalman gain is computed by the following equation [4]. 

K, = p:\prr 2.33 

where P^is the cross-correlation matrix between x^, and . The cross-correlation is 
defined later in the discussion below. To define the propagation equations, the following 
sigma points must be computed [4]. The filter starts by augmenting the state vector to L 
dimensions in the original state-vector, model noise, and measurement noise where L is 
the size of the vector x", or the augmented state defined by Equation 2.37 [9]. The 
covariance matrix is similarly augmented and this forms the augmented state estimate 
vector shown below. 
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2.34 


cr <— 2L columns from 



jr(o) 


XlH) = o-t(0 + x; 


2.35 

2.36 


where x“ is an augmented state defined by [4] 







Wk 






_^mxl _ 


Augmenting the eovariance requires the eomputation of 2(q+l) additional sigma points. 
It is important to mention here that q is the dimension of I is the dimension of , 
and m is the output dimension. While L is the size of the vector x“, the parameter yis 
given by the following [4]. 


Y = y/b + 2 2.38 

and the composite scaling parameter , A, is given by 

A = a\L+K-)-L 2.39 

The constant a, represents the spread of sigma points and is usually set to a small positive 
value (e.g., 1x10“''< a < 1). There are 2L values for cr^., each representing the 

positive and negative values of the square root. The Cholesky method is often used to 
find the square root of a matrix. Similar to the EKF, the UKF now propagates these 
sigma-points from a Gaussian distribution through a nonlinear function, and recreates a 
Gaussian distribution by calculating the mean and covariance of these results [11]. 
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y=f(x) 



Figure 6. Illustration of the unscented Kalman filter sigma-points propagation 


These sigma points are evaluated by: 




2.40 


where ^'^(0 is a vector of the first n elements of Xki^) is a vector of the next q 

elements of xl (0 ’ with 


Xk 


Xlii) 

x:ii) 

fkii) 


2.41 


The predicted mean for the state estimate is calculated using a weighted sum of points 
xL(i), given by: 


K..=f:^rxL(i) 


2.42 


where the weight terms 11/'"“" is given by: 
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L + A 


and 


-^^mean __ 


/I 


2(L + /l) 


/ = 1,2,...,2L 


9 •' -*-9 -^9 • • * 9 


2.43 


2.44 


Similarly, the predicted covariance term is given by: 


f.;,= f.wr [ j.',. (0 - i:., Jxuo - i;., ]' 

i=0 


2.45 


where the weight terms are given by 2.44, and the following equation. 


Wr =^^ + i^-cc" + j3) 

° L + A 


2.46 


The mean observation is given by 


2L 


y« = Z'r 


1=0 


where 


2.47 




2.48 


The output covariance matrix is given by: 


1=0 


2.49 


The innovations covariance is given by Equation 2.32. The cross correlation matrix is 
finally described as 


PS ,=Z w'"" [.Tw ('■) - ] [n., (i) - n., ]' 


2.50 
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A summary of these equations are listed in Table 5 and will be referred to in subsequent 
sections that describe the implementation of these filters. 
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Tables. 


Unscented Kalman Filter, from [9] 


Model 

=h(x,,u„v„A:) 

Initialize 

q(^o) = qo> 

P{K)-P, 

Gain 

K,=p,^{pry 

Update 

= x“ + 

p:=p,~-K,prKi 

^k^yk-y~k=yk-H^l,»kk) 

Propagation 

i-O 

p.;.=E K" [/:.,('■) - K,, ] [zu (0 - ]' 

y;., = (0 

(=0 

=i w','”" ('■) - y] h... (0 - yT 

_ pyy 

^k+\ ^k+\ 

C, = i; M',""' [y,., (0 - y^.,]" 


E, IMPLEMENTATION OF EKE AND UKF METHODS USING THE 
SIMPLE PENDULUM PROBLEM 

Prior to implementing the EKF and UKF on the spacecraft model, an easier 
problem was solved. For this, the simple pendulum was used. Figure 7 shows a diagram 
of the simple pendulum problem. 
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p 



Figure 7. Simple Pendulum Problem 
The dynamie equation is eommonly known and listed below. 


I +ml^ 

} 


Translated into the state spaee model, this becomes: 



2.55 


2.56 


2.57 
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We can now implement the state-space model into the simulation block diagram as our 
dynamics state function. The three states that were estimated were 0,9, and . 



Figure 8. Simulink Block Diagram of Simple Pendulum Model 


By solving Equation 2.57, we can then use its solution to determine our 
measurement equation. 


y^ea. = ^0 sin(^ + a) 


2.58 
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where represents the “measured” angle 0 and a represents some initial angular 

quantity. The measured values are then perturbed by white Gaussian random numbers to 
simulating sensor noise and are subsequently fed into both the EKF and UKF. Appendix 
A - Simple Pendulum Simulation, shows the details of the simulation, including the 
simulation blocks, and associated Matlab code. 

F, EKF AND UKF ESTIMATION RESULTS USING THE SIMPLE 
PENDULUM PROBLEM 

The results of this estimation problem show how the EKF does not estimate 
accurately for nonlinear problems. For the first simulation the pendulum was set to 0 = 
30°, 6= 0°/sec, and ly = 5 kg m . Figure 9 shows the 3a plot for the angle error between 
the estimated values and the true value. The 3a plot is typically used to the confidence 
interval of a given set of data. While the term “3 a” actually refers to three times the 
variance of the data distribution, mathematically 3a can be translated to mean that our 
data falls within approximately 99.73% of the symmetric confidence interval (Cl). 
Conversely, this means that approximately 0.27% of the data falls outside the CL The 
calculation for 3 a is shown below where the variance of diagonal values of the 
covariance matrix are used for n number of states. 



C,i 

C,2 




_ 

G,i 

G,2 



2.59 


_G.i 

G,2 




ih) = . 


ih), 

/ = 1 , 2 ,.., 

.,n 

2.60 


Here we can see that the EKF cannot accurately estimate the state due to the 
nonlinearity of the system. As the pendulum swings and the angle increases, the 
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nonlinearity of the dynamies increase and thus the filter becomes inaccurate. Conversely, 
we see in Figure 10 that the UKF accurately estimates the state well between the 3a 
bounds. 
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Figure 9. Angular Errors in EKF with 3a Error Bounds Simulation 1 (large 0) 
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UKF Angle Error (Deg) 
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Figure 10. Angular Errors in UKF with 3a Error Bounds Simulation 1 (large 0) 


Eigure 11 shows similar results for the estimation of the angular veloeity ro. 
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Eigure 11. Angular Rate Errors in EKE with 3a Error Bounds Simulation 1 (large 0) 
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Figure 12. Angular Rate Errors in UKF with 3a Error Bounds Simulation 1 (large 0) 


Furthermore, we ean see that the estimation for the moment of inertia, ly, is aceurate for 
both EKE and UKF. We ean conclude that this is largely because ly is a constant 
quantity. 


31 


















EKF Moment of Inertia Error 




1 

0.8 

0.6 

0.4 

0.2 

0 

- 0.2 

-0.4 

- 0.6 

- 0.8 

-1 










































— 


,| ^ 

V 

1 

\i 

■1 

- 

A,- 

■■ 

— 

---- 





— 









_ 


- 













lJ_ 









8 10 12 14 16 18 20 

Time (S) 


Figure 13. Moment of Inertia Errors in EKF with 3a Error Bounds Simulation 1 

(large 0) 
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Figure 14. Moment of Inertia Errors in UKF with 3a Error Bounds Simulation 1 

(large 0) 
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These plots clearly show how the UKF provides a more accurate solution for even 
simple nonlinear problems. To further verify this, a second simulation was performed 

using smaller initial conditions. Using 9-\\ 9-0, and fy = 5 kg rn , we can see the 

both filters estimate well within the 3a bounds. This can directly be associated with the 
small angle approximation where sin 6* = 6* for sufficiently small angles. These plots are 
shown below. 
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Figure 15. Angular Errors in EKF with 3a Error Bounds for Simulation 2 (small 0) 
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UKF Angle Error (Deg) 



Figure 16. Angular Errors in UKF with 3a Error Bounds for Simulation 2 (small 0) 


Similarly, as shown below, we ean see that the angular rates also fall within the 
bounds. 


EKF Angular Rate Error (Deg/Seo) 



Figure 17. Angular Rate Errors in EKF with 3a Error Bounds for Simulation 2 

(small 0) 
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UKF Angular Rate Error (Deg/Sec) 



Figure 18. Angular Rate Errors in UKF with 3a Error Bounds for Simulation 2 

(small 0) 


We ean also see that the both moments of inertia also converge the proper values. 
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Figure 19. Moment of Inertia Errors in EKF with 3a Error Bounds for Simulation 2 

(small 0) 
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UKF Moment of Inertia Error 
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Figure 20. Moment of Inertia Errors in UKF with 3a Error Bounds for Simulation 2 

(small 0) 
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III. IMPLEMENTATION OF EKE AND UKF FOR SPACECRAFT 

ATTITUDE DETERMINATION 

A, GENERALIZATIONS 

While Chapter II diseusses the fundamentals of both EKF and UKF, this ehapter 
deseribes the implementation of both methods for attitude determination. In order to 
perform Kalman filtering for attitude estimation we must first examine the nature of 
quaternion estimation. The following diseusses the analytieal modeling setup, basie 
quaternion attitude kinematies, and finally, the implementation of both EKF and UKF 
filters for spaeeeraft attitude estimation. 

B, ANALYTICAL MODELING AND SETUP FOR ATTITUDE 

DETERMINATION SIMULATIONS 

1, Background 

To implement the Kalman filters, a spaeeeraft simulation was ereated in 
MATFAB Simulink. Mueh of the initial foundation for this simulation was built 
previously, and is doeumented in [14] and [4]. For a better understanding of how the 
simulation works, the following seetions will briefly diseuss the several of the general 
Simulink Bloeks. For our purposes, we will define the general simulation bloeks as the 
following 

• Orbit Propagator 

• Environmental Effeets 

• Dynamies and Kinematies 

• Disturbanee Torques 

• Sensors and Noise Modeling 

Partieularly, the bloeks that will be diseussed are Spaeeeraft Kinematies, Attitude 
Disturbanee Torques, and Spaeeeraft Sensor/Noise Modeling. These bloeks, and the 
entire simulation are shown in Appendix B - Spaeeeraft Attitude Determination 
Simulation. 
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2. Dynamics and Kinematics 

Shown in Figure 21, the Dynamics block calculates the spacecraft angular body 
rates along each body axis by integrating applied forces, including control torques, based 
on Euler’s equations [15]. 
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Figure 21. Attitude Dynamics and Kinematics Simulink Block 


The Euler equations are listed below [15]. 


CO, = 


T,-{J,-Jy)co,co^ 


J, 


COy = 


J, 


CO, = 


T,-{Jy-J,)cOyCO^ 


J, 


2.61 


These angular rates were then integrated in to the Kinematics block to determine 
the spacecraft orientation. For these simulations, the orientation is described in 
quaternions. The quaternion kinematic differential equation is: 
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3, Spacecraft Attitude Disturbance Torques 

Three major torque disturbanees were taken into eonsideration for this simulation, 
gravity gradient, aerodynamie torque, and solar torque. For gravity gradient torque, the 
following equations were used. 


where 


Z 


GG 


3ju 


( *^2 “ ) ^ 2^3 

( -^y ~ "^x ) 


2.63 


Cl 


'O' 

^2 

II 

O 

0 
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The aerodynamie torque was ealculated using the simple drag equation. 


2.64 


Faer0=-PV'C,A 


2.65 


Here we should note that the veloeity was determined from the orbit parameters, the 
eoeffieient of drag (Cd) was assumed to be 2, and effeetive area (A) was determined 
using spaeeeraft eomponent areas and eenters of pressure. Finally, solar torque was 
determined using a Simulink bloek diagram shown in Appendix B. 
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4, Spacecraft Sensor and Noise Modeling 

All of sensor modeling done for this simulation was completed in Reference [4], 
where the author accurately modeled sensor sampling rates and noise sources based on 
manufacturer specifications. We can see this in the “Sensors Block” of the simulation. 
The most important information from the previous work is shown in Table 6, which 
shows the example of noise coefficients for the gyro. These numbers are implemented 
into the spacecraft simulation gyro random noise modeler. 


Table 6. Summary of Gyro Noise Coefficients, from [4] 



E[nl~] (°Wsec) 

(°Wsec') 

Gyro Data 1 

7.840e-04 

1.440e-07 

Gyro Data 2 

7.840e-04 

3.240e-08 

Gyro Data 3 

7.840e-04 

3.240e-08 


The equation for modeling the internal measurement unit (IMU) is listed below. 

2.66 

2.67 


®(0 = ry(0+A0+^v(0 

m=ut) 


where a)(t) is the continuous-time measured angular rate, and ri^{t) and are 

independent zero-mean Gaussian white-noise processes. 
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C. ATTITUDE KINEMATICS FOR QUATERNION ESTIMATION 

This section describes the Kalman filter as it applies to attitude estimation. It is 
important to note that the equations found in this section apply to both the extended and 
the unscented Kalman filters. 


The quaternion is defined in Equations 2.68, 2.69, and 2.70. 

q = 94 7 

q = [q^ q^'f 


q, = cos(-f-) 


2.68 

2.69 

2.70 


where q is the quaternion, e is the Euler’s axis, 0 is the Euler’s angle, and the quaternion 
follows the normalization of q^q = 1. The attitude matrix can be related to the quaternion 
by the equation below [2]. 

A(q) = S"(q)'P(q) 2.71 


where S^(q) and T^(q) are defined by Equations 2.72 and 2.73. 


S"(q)- 


94^3x3 


94^3x3 


2.72 

2.73 


Here is a 3x3 identity matrix and [qx] is the cross product matrix described below. 
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-^3 
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2.74 


[^x] 
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q, 

-Qi 


-^1 
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Notably, the quaternion error eannot aeeurately found by subtraction, as the result would 
not satisfy the unit norm constraint, and a renormalization would be needed. The 
multiplicative error is defined as [16]; 

(5q = q ® q~' 2.75 

Here we use the symbol ® to indicate the quaternion multiplication [2]. This relationship 
is described in Equation 2.76. 


A(q')A(q) = A(q'Oq) 2.76 

For implementation, the function XI was used in Matlab. This can be seen in Appendix 
A - Matlab Code and Simulink Diagrams. The time derivative of the quaternion error 
becomes 


Jq = q(H)q ' + q(H)q ' 2.77 


As derived in [16], the estimated quaternion kinematics equation is given by 


q(0 = 7S[q(0](o(0 


2.78 


Where o)(0 is the 3x1 angular velocity vector. The local error-quaternion, 
5q = \^5g^ can now be used to find the generalized Rodriguez parameter which will 

be useful later in the implementation of the UKF [4]. 
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2.79 


5v = f 


5 ^ 

a + Sq^ 


where a is a parameter from 0 to 1, and / is a scale factor. Suggested values for, /, is 

2(a+l) so that ||^p|| is equal to ,9 for small errors, where ,9 is the angle of rotation [4]. 

In the simulations presented in the thesis a was set to 1 to reproduce the results shown in 
[4]. While the propagation of the state and covariance can be accomplished by using 
numerical integration techniques, the measurement observations are typically sampled at 
higher rates than they are updated. This proves useful, as we can use a discretized 
version of the propagation equations. Using the power series, we can derive the new 
discretized propagation equation from 2.78 [9]. 




k=0 
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( 2 *)! 
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2k+l 1 


{2k + \)l 


2.80 


Using the identities described in Equations 2.81 and 2.82, we can substitute them into 
Equation 2.80. 

= 2.81 


= (-!)'= 1^1"'Q(^) 


2.82 
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2.83 


This equation then simplifies using trigonometric identities to the following 
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2.84 
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Finally, the quaternion propagation is found to be [9]; 


q,,i =Q(®;)q 


2.85 


Where al and are the post-update estimates and are given by Equations 2.86 

and 2.87. 
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2.87 


For both the EKE and the LIKE, we will use a rate gyro and a magnetometer. Given a 
post-update estimate for the bias ^, we will use the following equation to find the post¬ 
update angular veloeity and propagated bias. 




2.88 
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We will use these equations in both the EKF and UKF to solve the attitude determination 
problem in the following seetions. 

D. CRASSIDIS AND MARKLEY’S UNSCENTED QUATERNION 

ESTIMATOR (USQUE) 

In this seetion, the unseented Kalman filter deseribed in Crassidis and Markley’s 
paper on spaeeeraft attitude estimation, Referenee [4], is implemented. This filter is 
ealled the UnSeented QUatemion Estimator (USQUE). More speeifieally, the following 
deseribes how the USQUE is implemented in spaeeeraft attitude-determination 
simulations using MATLAB. 

First, however, we must take step baek and look at implementation of the UKF 
proeess as a sequential series of steps. Figure 22 shows the UKF graphieally in the form 
of a flow ehart. Similarly, we will refer to this flow ehart throughout this section as it 
follows Crassidis and Markley’s USQUE closely. The Matlab Code generated for this 
simulation also follows this flowchart and is listed in Appendix A. 
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Sensor Inputs and 
Estimated 
Environment 
Information 



• Calculate Variance for Sensors 

• Assume initial zero bias 

• Use Sensor information for 
quaternion and rate initialization 


• Use Choiesky Decomp to Find First 
Sigma Points 

• Find the Error Quaternion (Ref [4] 
Eq. 33) 

• Propagate the Updated Quaternion 
(Ref [4] Eq. 34) 

• Propagate the Updated Bias (Ref [4] 
Eq. 35) 

• Calculate Mean Points (Ref [4] Eq. 7 
& 9, Ref [9] Eq. 5.290 & 5.292) 


• Calculate Covariances Terms Pxx, Pyy, 
and Pxy (Ref [4] eqs. 8, 11, and 13) 

• Caicuiate Gain (Ref [4] 4) 


• Error Covariance Update 

• State Update 

• Calculate the Updated Error 
Quaternion (Ref [4] 45) 

• Calculate the Final Updated 
Quaternion (Ref [4] 44) 


Finally the error quaternion, del_p, is 
set to zero and the process is “reset” 
until the next sensor update. 


Figure 22. Unscented Quaternion Estimator Flow Chart 
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To begin, Figure 23 shows the UKF bloek of the attitude determination simulation 
shown in Appendix A. 

Table 7 deseribes the inputs and outputs of this bloek. 
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-^<^[Pdiag_u] 


-^<;^Pnorm_u] 


Unscented Kalman Filter 


Figure 23. Unseented Kalman Filter Bloek - Level 1 


Table 7. Deseription of Inputs and Outputs for UKF Bloek - Level 1 


Input 

Output 

Variable Name 

Description 

Variable Name 

Description 

w_BNm, () 

Sensor measured angular 

rate (from gyro) 

w_BNf_u, ((U ) 

Estimated angular 

rate 

Bm, (A) 

Sensor measured magnetic 

field (from magnetometer) 

q_BNf_u, (q ) 

Estimated quaternion 

b,(A) 

Estimated magnetic field 

from environment model. 

bias_f_u 

Estimated 

magnetometer bias 



Pdiagu 

Diagonal terms of the 

covariance matrix 



Pnorm u 

Norm of the 

covariance matrix 
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As Figure 23 shows the top level of the UKF, Figure 24 shows the Level 2 bloek 
showing a few more inputs. These will be diseussed further in the seetion. As a side 
note, the following seetions are also well doeumented in the embedded Matlab eode 
assoeiated with this Simulink bloek. This eode ean be found in Appendix B. 



Figure 24. Unseented Kalman Filter Bloek - Level 2 

1. Initialization 

Referring to Figure 22, we ean see that the USQUE proeess begins with the 
initialization portion of the estimation. Here we must ehoose the initial values for our 
states, whieh inelude the quaternion and the bias. For the initial simulations, the initial 
quaternion was set to [0,0,0,1] and the bias to [0,0,0]. For later simulations, as diseussed 
in the results seetion, initial eonditions were ehanged to highlight major differenees 
between the UKF and EKF. 

2, Calculation of Sigma Points 

Caleulations of Sigma Points begin with defining the following state veetor: 
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Xki^) = xl = 


k 


2.89 


Here we ean use Equation 2.79 for 5 ^]., whieh is the 4x1 error quaternion, and the 3 

state bias term, k ■ These values will be propagated and used to update the final nominal 

state. This resulting covariance matrix is a 6 x 6. It is important to note here that for 
propagating these values forward we can now use Equations 2.42 through 2.50. 
However, before we use these equations, we must partition the sigma points Xki^) so that 
we can work only with the quaternion portion. 


lkii) = 



1 = 0,1,...,12 


2.90 


where xf^is the attitude error part, and xf(0 is the gyro bias part. Now that we have 

parsed out these terms, we must determine the new quaternion generated by multiplying 
the error quaternion by its current estimate. 

q:(0) = q; 2.91 

q;(0 = ^q:(0®q: / = 1,2,...,12 2.92 


where is the current quaternion estimate, and (Jq^ is the error quaternion. The error 
quaternion is broken up into the 3 state quaternion vector and the forth 

quaternion, , shown in Equations 2.93, 2.94, and 2.95. 




i = l,2,...,12 


2.93 
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2.94 


Sq^ii) 


-a 

|x?(i)| 


f+il 

2\ 
-a ) 

xf (0' 

f + 

X^O') 

2 


Sqld) = /-‘ [a + Sql^ (/)]xf (/), / = 1,2,...,12 


2.95 


We chose / = 2{a + 1), where a values were selected using Table 1 from [4]. Next, these 
updated quaternions are propagated forward using Equation 2.85 for each i, or step, 
shown below. 

q-,,(0 = Q(a>;(0)q:(0 i = 0,1,...,12 2.96 

where again the angular velocities are given by Equation 2.97 similar to Equation 2.88 in 
the previous section. Here, we can see that for - xf (0) ^ xf (0) = • 

®;(0 = 4-xf(0, i = 0,l,...,12 2.97 

The propagated error quaternions are now calculated using Equation 2.98. 

SqlAi) - q^u(0 ® [q^.i(0)]‘', i = 0 , 1 ,..., 12 2.98 

it is interesting to note that where <7q^^j(0) here should be the identity quaternion 
[0, 0, 0, 1]. Einally, the propagated sigma points can be calculated using the following 
equations. 






tUi) = f 


a + 5q.{i) 


, / = 1 , 2,...,12 


where and Sq^ (i) are found from the following equation. 


2.99 

2.100 
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2.101 



3, Covariance and Gain Calculations 

The next step in the UKF proeess is to ealeulate the covariances and gains which 
is the most notable difference between the EKF and UKF. Now that we have calculated 
our sigma points, we can determine these values. As previously mentioned in the 
derivation of the UKF, we can determine the predicted covariance matrix , shown as 

Pxx in the Matlab code, the output covariance P^y , and the cross correlation covariance 
P^ly. These equations are found as 2.39, 2.43, and 2.45, respectively. These equations 
are utilized in the “Covariance and Gain Calculations” section of the embedded Matlab 
code for the UKF. For initial conditions, Pxx is the set to 2* , where the variations for the 
sensors are used. The following equation is used for . 


- 03,3 

"2 O3 3 (J^L 3 

L 3x3 u 3x3 J 

The mean observation is also needed to calculate the covariance terms. 

A[q^(0]ri 

^[ q ^( 0]^2 

^[q^(0]fv 




2.103 


2.104 
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With these covariance matrices calculated, we can now determine the Kalman gain K 
from the equation in Table 5. Unscented Kalman Filter, from [9]. This equation is also 
shown below. 


K ^ = p ^{ p ;;'^ y ' 2.105 


4. Update Routine for States and Error Covariance 

After the gains are calculated, states and error covariances must be updated. First, 
the error covariance is updated using the following equation. 


P:=P,-K,PrKl 2.106 


The state update is found using 


A -L A _ 


2.107 


Finally we can update the quaternions using the following set of equations where 








2.108 




■4+1 ^*^4. 


2.109 


=■ 


-«||8pl+i + f4f + 8p 




f + 8p;, 


2.110 




2.111 
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These equations are very similar to those used earlier to find the initial error quaternion. 
The final step here is to update the bias using Equation 2.88. For further clarification, the 
Matlab code references the equations used with respect to [4]. This unscented Kalman 
filter was built to be compared with the extended Kalman filter. The EKF and LIKE 
models for attitude are based on the model presented in [9] and [4] where the state vector 
is represented as the error in the quaternion and generalized Rodriquez parameter 
respectively. 

E, IMPLEMENTATION OF THE EXTENDED KALMAN FILTER 

The EKF implemented in this simulation uses many of the equations used in 
previous sections. Similar to the previous section, Figure 25 shows a flow chart of the 
EKF. By comparison, we can see very clearly that the major difference in the EKF is the 
calculation of the sensitivity matrix, which is the inherent linearization processes 
associated with this filter. Much of the information on the derivation of the EKF is 
discussed in [4] and Table 8 shows a summary of the EKF equations. A complete listing 
of both the Matlab Simulation block diagram and embedded Matlab code are shown in 
Appendix B. 
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Sensor Inputs and 
Estimated 
Environment 
Information 



Calculate Variance for Sensors 
Assume Initial Zero Bias 
Use Sensor Information for Quaternion 
and Rate Initialization 


Calculate the Estimated Quaternion 
(Ref [9] 7.18) 


Calculate the Covariance (Ref [9] 7.57, 
7.58, 7.59, and 7.60) 


Calculate the Sensitivity Matrix (Ref [9] 
7.43) 

Calculate Gain (Ref[4]) 


Update Covariance (Ref [9] 5.44) 
Update Bias (Ref [9] 7.46) 
Calculate the Updated Error 
Quaternion (Ref [9] 7.44) 
Calculate the Final Updated 
Quaternion (Ref [9] 7.487) 


Finally the error quaternion, .5*8a, is 
set to zero and the process is “reset” 
until the next sensor update. 


Figure 25. Extended Kalman Filter Flow Chart 
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Table 8. 


Summary of EKF Equations, from [9] 
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IV. COMPARISON OF SIMULATION RESULTS USING EKF 

AND UKF FILTERING METHODS 

A, SIMULATION CONDITIONS 

In this section, several performance comparisons between the USQUE and EKF 
are made through simulations using the previously discussed spacecraft model and the 
designed EKF and UKF filters. Using a 500-km circular orbit the simulation time was set 
at 4,000 seconds. The attitude determination hardware in these simulations consisted of a 
gyroscopic rate sensor and a three-axis magnetometer (TAM). The magnetic field 
reference model uses a magnetic dipole approximation as previously discussed. 
Furthermore, these sensors were characterized in previous work [4]. In the first 
simulation, the initial attitude error was set only to 30°, while the attitude rate error was 
set to 0°/sec in all axes. A second simulation was run using an initial attitude error of 30° 
and an attitude rate error of 30°/sec in all axes. 

B. SIMULATION 1 RESULTS 

The following shows the results of Simulation 1. Figure 26 shows the attitude 
error of the quaternion for the EKF estimator with Scr bounds. We can see that the EKF 
takes approximately 4,000 seconds before the error is bounded. Conversely, we can see 
that the attitude error of the UKF is bounded in approximately 2,500 seconds. This is 
shown in Figure 27 where the generalized Rodriguez parameters are shown. It is 
important to note that we use the generalized Rodriquez parameters instead of the 
quaternion for the UKF because the Scr bounds are calculated from the square root of the 
diagonals of the covariance matrix P . For the UKF the covariance matrix is built from 
(^p which is shown in Equation 2.79, as is the error in the vector of the generalized 
Rodriquez parameter. For comparison purposes. Figure 30 will show both normalized 
quaternion errors without the 3cr bounds. 
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Figure 26. Simulation 1 Quaternion Attitude Error with 3a Bounds for EKF 
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Generalized Rodriquez Parameter Error for UKF 
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Bounds for UKF 
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Similarly, we see that both EKF and UKF bias errors eonverge in a similar way. 
While Figure 28 shows the bias for the EKF eonverging within the 3a bounds at 
approximately 2,700 seconds. Figure 29 shows convergence at a little after 1,000 
seconds. We can also see that the initial estimates of the EKF are more inaccurate than 
the UKF. 
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Figure 28. Simulation 1 EKF Bias Errors with 3a Bounds 
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Figure 29. Simulation 1 UKF Bias Errors with 3a Bounds 
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Finally, we can see that the normalized EKF and UKF attitude errors converge as 
originally predicted and demonstrated in the simple pendulum problem. Figure 30 
clearly shows the better performance of the UKF. Again, as a nonlinear estimator, the 
UKF consistently shows better performance on all figures. This is again shown in 
comparison plot of the normalized bias errors displayed in Figure 31 where we can see 
that, although both estimators are trending appropriately, that the UKF performs 
significantly better. 
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Figure 30. Comparison of EKF and UKF Normalized Attitude Errors for 

Simulation 1 


62 














































Normalized EKF and UKF Bias Errors 



Figure 31. Comparison of EKF and UKF Normalized Bias Errors for Simulation 1 


C. SIMULATION 2 RESULTS 

The seeond simulation shows very similar results. Although we ean see similar 
trends in both the EKE and UKF error estimates, we ean see that the UKF eonsistently 
performs better in every plot. Again, Figure 32 shows the EKF attitude quaternion error, 
whieh settles within the 3a bounds at approximately 3,000 seeonds. Figure 33 shows the 
UKF attitude generalized Rodriquez parameter error settles at 2,500 seeonds. The 
inereased performanee is shown without fail for all subsequent plots in this seetion. 
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Figure 32. Simulation 2 Quaternion Attitude Error with 3a Bounds for EKF 
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Generalized Rodriquez Parameter Error for UKF 





Figure 33. Simulation 2 Generalized Rodriguez Parameter Attitude Error with for 3a 

Bounds for UKF 

Figures 34 and 35 show that the bias for the EKF settles at approximately 2,700 seconds 
while the UKF bias settles at 1,700 seconds. 
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Figure 34. Simulation 2 EKF Bias Errors with 3a Bounds 
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Figure 35. Simulation 2 UKF Bias Errors with 3a Bounds 
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Finally, we can see in Figure 36 that the normalized attitude error of the UKF is 
much better. Similarly, this is shown in Figure 37 with the comparison of the normalized 
bias errors. 



Figure 36. Comparison of EKF and UKF Normalized Attitude Errors for 

Simulation 2 
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Normalized EKF and UKF Bias Errors 



Figure 37. Comparison of EKF and UKF Normalized Bias Errors for Simulation 2 


D. DISCUSSION OF RESULTS AGAINST PREVIOUS LITERATURE 

Much of the work on the UKE was researched [4]. In this paper, Crassidis and 
Markley discuss the performance of the UKE as it applies to the spacecraft attitude 
determination problem. Eigure 38 is taken from [4] and shows many similarities to the 
spacecraft model designed for this thesis. Although the results are not identical, they 
shown very similar trends and performance characteristics. It should be noted that the 
initial error conditions used for simulations in [4], were much larger and are used here to 
highlight the differences in accuracy. The simulations done for this thesis were set at 
4,000 seconds. 
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38. Norm of Attitude Errors, from [4] 


V. CONCLUSION 


A, SUMMARY 

The results from the simulations clearly show that the UKF developed here is 
more accurate than the EKF [2], Both the EKF and UKE were rigorously tested and 
validated against previous research papers. These results show both that the UKE is 
largely better for nonlinearities, but that the EKE performs rather well. To take 
advantage of the UKE, large nonlinearities must be present in the physical dynamics of 
the system. In summary, we have shown that the UKE has a lower expected error than 
the EKE for all instances of spacecraft attitude determination. We showed, in the 
pendulum problem, that as the nonlinearity of the dynamics increase, that the UKE shows 
increased performance over the EKE. However, for slightly nonlinear or linear 
estimation, the EKE performs well and will provide accurate solutions. The one 
remaining question is the computational expense that the extra computations cost. In our 
simulations the UKE performed approximately 2.4 times slower than the EKE, which was 
consistent with [4]. As the optimization of any process is measured by a cost function, 
one must evaluate and prioritize the resources available. Eiterature tells us that the UKE 
has 2.5 times the cost in computational time of the EKE [4]. Eor spacecraft with relaxed 
attitude-control requirements and low computational power, it could be argued that the 
EKE could perform sufficiently without the added expense. However, the UKE can 
certainly be used in the worst case conditions, such as partial loss of attitude control, and 
in the “lost in space” scenario where anomalies in the separation event from the launch 
vehicle imparts a large torque on the spacecraft hurling it into an unwanted orientation. 
These scenarios, although somewhat unlikely, mostly likely cannot be recovered from 
using an EKE estimator. 

B, FUTURE WORK 

This thesis presents partial validation of the UKE and EKE estimators. Although 
the results are favorable and largely resemble other research papers, a more realistic 
simulation would require hardware. Eurther developments in the model can also be 
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applied. A high order magnetie field model eould be implemented if the eomputing 
resourees were available. Most importantly, Monte Carlo simulations should be run to 
show the full performanee eharaeteristies of both filters. All of this work is eompletely 
possible for further thesis students and laboratory researeh sueh as the eurrently being 
performed in the Nanosatellite Advanee Coneepts Laboratory. 
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Model Initialization Parameters 


time_step=0.05; 
sigmanoise=le-2; 
R=sigmanoise^2; 
Q=diag([0.00001,0.00001,0.001]); 


fncA.m 


function deriv = fncA(x) 
m=50; 
g=9.81; 

1 = 0 . 1 ; 

deriv=zeros (3,1) ; 
deriv (1, 1) =x (2, 1) ; 

deriv(2,l)=-(m*g*l*sin(x(l)))/(x(3)+m*1^2) 


fncC.m 


%#eml 

function y = fncC(x) 
Bo=0.5; 

alf=30*pi/180; 
y=Bo*sin(x(l,l)+alf); 
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Unscented Kalman Filter - Embedded Matlab Block 


%#eml 

function [x_kl,Pxx_kl] = UKF(x_k,Pxx_k,Y_meas,ts,Q,R,kappa) 
% This block supports the Embedded MATLAB subset. 

% See the help menu for details. 

Dx=size(x_k,1); 

Dy=size(Y_meas,1); 

NSig=2*Dx+l; 

sig_x=(chol((Dx+kappa)*Pxx_k))'; 

9 - 9 - 9 - 9 - 9 - 9 - 9 - 9 - 9 - 9 - 9 - 9 - 9 - 9 - 9 - 9 - 9 - 9 - 9 - 9 - 9 - 

000000000000000000000 

x_sig_k=x_k*ones(1,NSig)+[zeros(Dx,1) sig_x -sig_x]; 
RK=zeros(Dx,4); 
x_sig_kl=zeros(Dx,NSig) ; 
y_sig_kl=zeros(Dy,NSig) ; 

for i=l:NSig 

RK(:,1)=fncA(x_sig_k(:,i)); 

RK(:,2)=fncA(x_sig_k(: , i)+l/2*ts*RK(:,!)); 

RK ( :,3)=fncA(x_sig_k(:,i)+l/2*ts*RK(: , 2) ) ; 

RK(:,4)=fncA(x_sig_k(:,i)+ts*RK(: , 3)) ; 
x_sig_kl(:,i)=x_sig_k(:,i)+1/6*ts*RK*[1 2 2 1] ' ; 
y_sig_kl(:,i)=fncC(x_sig_kl(: , i)); 

end 

W=ones(NSig,1)/(2*(Dx+kappa)) ; 

W(1, 1)=kappa/(Dx+kappa); 

x_klp=x_sig_kl*W; 

y_klp=y_sig_kl*W; 

Pxx_klp=Q; 

Pyy_klp=R; 

Pxy_klp=zeros(Dx,Dy); 
for i=l:NSig 

xdif=x_sig_kl(: , i)-x_klp; 
ydif=y_sig_kl(:,i)-y_klp; 

Pxx_klp=Pxx_klp+xdif*xdif'*W(i,1); 

Pyy_klp=Pyy_klp+ydif*ydif'*W(i, 1) ; 

Pxy_klp=Pxy_klp+xdif*ydif'*W(i, 1) ; 

end 

K=Pxy_klp/Pyy_klp; 

Pxx_kl=Pxx_klp-K*Pxy_klp'; 
x_kl=x_klp+K*(Y_meas-y_klp); 


75 



Extended Kalman Filter Block 



Extended Kalman Filter Block-Update Block 



x_est_ukf1 
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Integrate- Embedded Matlab Block 


function [xprop. Phi] = Integrate(x,ts) 

% This block supports the Embedded MATLAB subset. 
% See the help menu for details. 


KXl=fncA(x); 

KP1=F(x); 

KX2=fncA(x+1/2.0*ts*KXl); 

KP2=F(x+1/2.0*ts*KXl)*(eye(3)+1/2.0*ts*KPl); 
KX3=fncA(x+1/2.0*ts*KX2); 

KP3=F(x+1/2.0*ts*KX2)*(eye(3)+1/2.0*ts*KP2); 
KX4=fncA(x+ts*KX3) ; 

KP4=F(x+ts*KX3)*(eye(3)+ts*KP3) ; 

xprop = X + 1/6.0*ts*(KX1+2*KX2+2*KX3+KX4); 

Phi = eye(3) + 1/6.0*ts*(KP1+2*KP2+2*KP3+KP4); 
return 


function deriv=F(x) 

m=50; 

g=9.81; 

i=.i; 

deriv=zeros(3,3) ; 
deriv(1,2)=1; 

deriv (2,1)=-m*g*l*cos(x(l,l))/(x(3,l) +m*1^2) ; 
deriv (2,3) =m*g*l*sin (x(l,l)) / (x(3,l) +m*l''2 ) ''2 ; 
return 


pendulum plots.m 

x_true=zeros; 

[m n p]=size(x); 

x_true(1:m,1:p)=x(l:m,l,l:p) ; 

x_true=x_true'; 


%% State 1: Theta 

%EKF Error 
figure (1) 

plot (t, (x_est_ekf(:,1)-x_true(:,1))*180/pi); 
grid on 

title ('EKF Angle Error (Deg) ') 

xlabel('Time (S) ') 

ylabel (' Angle Error (Deg) ') 

% ylim([-5 5]) 
hold on 

plot(t, 3*sqrt(Pxx_est_ekf(:,l))*180/pi', '".'); 
plot (t, -3*sqrt(Pxx_est_ekf(:,l))*180/pi', '-.'); 
%UKF Error 
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figure (2) 

plot(t, (x_est_ukf(:,1)-x_true(:,!))*180/pi); 
grid on 

title ('UKF Angle Error (Deg) ') 

xlabel( 'Time (S) ') 

ylabel (' Angle Error (Deg) ') 

% ylim([-5 5]) 
hold on 

plot(t, 3*sqrt(Pxx_est_ukf(:,l))*180/pi', '-.'); 
plot (t, -3*sqrt(Pxx_est_ukf (:,l))*180/pi','-.') 

%% State 2: Omega - Angular Rate 

%EKF Error 
figure (4) 

plot(t, (x_est_ekf(:,2)-x_true (:,2))*180/pi); 
grid on 

title ('EKF Angular Rate Error (Deg/Sec) ') 
xlabel ('Time (S) ') 

ylabel (' Angular Rate Error (Deg/Sec) ') 

% ylim([-10 10]) 
hold on 

plot(t, 3*sqrt(Pxx_est_ekf(:,2))*180/pi', '-.'); 
plot (t, -3*sqrt(Pxx_est_ekf (:,2))*180/pi','-.') 

%UKF Error 
figure (5) 

plot (t, (x_est_ukf(:,2)-x_true(:,2))*180/pi); 
grid on 

title ('UKF Angular Rate Error (Deg/Sec) ') 
xlabel ('Time (S) ') 

ylabel (' Angular Rate Error (Deg/Sec) ') 

% ylim([-10 10]) 
hold on 

plot(t, 3*sqrt(Pxx_est_ukf(:,2))*180/pi', '-.'); 
plot(t, -3*sqrt(Pxx_est_ukf (:,2))*180/pi','-.') 

%% State 3: ly - Moment of Inertia 

%EKF Error 
figure(7) 

plot(t, x_est_ekf(:,3)-x_true(:,3)); 
grid on 

title ('EKF Moment of Inertia Error') 
xlabel ('Time (S) ') 

ylabel (' Moment of Inertia (kg m''2) ') 

% ylim([-5 5]) 
hold on 

plot(t, 3*sqrt(Pxx_est_ekf(:,3))', '-.' ); 
plot(t, -3*sqrt(Pxx_est_ekf(:,3)) ' , '-. ' ) ; 

%UKF Error 
figure(8) 

plot(t, x_est_ukf(:,3)-x_true(: , 3)) ; 
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grid on 

title ('UKF Moment of Inertia Error') 
xlabel('Time (S) ') 

ylabel (' Moment of Inertia (kg m\''2) ') 

% ylim([-5 5]) 
hold on 

plot(t, 3*sqrt(Pxx_est_ukf(:,3))', '-.' ); 
plot(t, -3*sqrt(Pxx_est_ukf(:,3)) ' , '-. ' ) 
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APPENDIX B - SPACECRAFT ATTITUDE 
DETERMINATION SIMULATION 


ADS SpacecraftSim.mdl 


I J_Matrix~ D-►<&; Matrix ] I 

Interia Matrix 


-►<Jw_BNo] 

Initial Inertial Body Rates 
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Orbital in Intertial Rates 
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[w_8No]^- 
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Dynamics and Kinematics 
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Unscenled Kalman Filter 
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Orbit Propagator-Simulink Block 
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-X m 
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Calculate Altitude-Simulink Block 
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Keplerian Orbit Propagation- Simulink Block 
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Calculate Orbital Elements - Embedded Matlab Code 


function [ rho_earth,beta,TA,r,Vt,Vr, Lat, Long] = 
fen(time. Re, incl,ecc,R,V) 

"6 

"6 

% This function computes the classical orbital elements 
% from the state vector (R,V) using Algorithm 4.1. As well as 
% other orbital parameters needed by the model. 

"6 

% mu - gravitational parameter (m''3/s''2) 

% R - position vector in the geocentric equatorial frame (m) 

% V - velocity vector in the geocentric equatorial frame (m/s) 
% r, V - the magnitudes of R and V 
% vr - radial velocity component (m/s) 

% H - the angular momentum vector (m''2/s) 

% h - the magnitude of H (m“2/s) 

% incl - inclination of the orbit (rad) 

% N - the node line vector (m“2/s) 

% n - the magnitude of N 
% cp - cross product of N and R 

% RA - right ascension of the ascending node (rad) not used 

■k-k-k-k-k-k-k-k-k-k-k-k-k-k 

% E - eccentricity vector 
% ecc - eccentricity (magnitude of E) 

% eps - a small number below which the eccentricity is 
% considered to be zero 

% w - argument of perigee (rad) not used 

kkkkkkkkkkkkkkkkkkkkkkkkkkkkkkk 

% TA - true anomaly (rad) 

% Vt - tangential velocity (m/s) 

% Vr - radial velocity (m/s) 

% rho_earth - earth anglular radius 
% beta - beat angle (rad) 

% Lat - Latitude of satellite (rad) 

% Long - Longitude of satellite (rad) 

"6 

mu = 398.6004418el2; % m''3/s''2 

eps = l.Oe-10; 
r = norm(R); 

V = norm(V); 
vr = dot (R, V) /r; 

H = cross(R,V); 
h = norm(H); 

% Calc inclination 

% { 

c = H(3)/h; 

if (c < -1) && (c > 1) 
c = c - pi; 

end 

incl = acos (c); 

%} 

% Calc right ascension of the ascending node (rad) 

N = cross([0 0 1],H); 
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n = norm(N); 


% Calc Eccentricity 

E = 1/mu*((v^2 - mu/r)*R - r*vr*V); 
%ecc = norm(E); 

% True Annomoly 
if ecc > eps 

c = dot(E,R)/ecc/r; 
if (c < -1) && (c > 1) 
c = c - pi; 

end 

TA = acos(c); 
if vr < 0 

TA = 2*pi - TA; 

end 

else 

cp = cross(N,R); 
c = dot(N,R)/n/r; 
if (c < -1) && (c > 1) 
c = c - pi; 


end 


if cp(3) 

>= 0 

TA = 

acos (c); 

else 


TA = 

2*pi - a 

end 



end 

% Calculate the tangential and radial velocities 
Vt = h/r; 

Vr = mu/h*ecc*sin(TA); 

% Calculate earth angular radius 
rho_earth = asin(Re/r); 

% Beta calcs 
wb_0 = 0; 
ub_0 = 0; 

wb_dot = (-9.9639/86400) *rho_earth''(3.5) *cos (incl) / (l-ecc''2) ^2; 

wb = (wb_0 + wb_dot*time)*pi/180; 

gamma = 23.442*pi/180; %rad 

ub_dot = (0.985648/86400)*pi/180; %rad 

ub = ub_0+ub_dot*time; 

beta = asin(sin(ub)*sin(gamma)*cos(incl) + ... 

cos(ub)*sin(incl)*sin(wb)-sin(ub)*cos(gamma)*sin(incl)*cos (wb)); 

% For Mag Calc - 

% Calculate Earth Coordinate by Simulate the Earth's Rotation 
% Track the movement of (0 Lat, 0 Long) 

PhiE = time*2*pi/(23.93*3600); 

ThetaE = 0; 
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EarthCoord = [Re,ThetaE,PhiE]; 

% Calculate Sat Coordinate In Polar 
X = R(l); 

Y = R(2); 

Z = R(3); 

Theta = atan2(sqrt(X^2+Y^2) , Z) ; 

Phi = atan2(Y,X); 

SatCoord = [r,Theta,Phi]; 

% Calculate Lat and Long 
ThetaO = pi/2-SatCoord(2); 

PhiO = SatCoord(3); 

Phil = EarthCoord(3); 

Lat = ThetaO; 

Long = (PhiO-Phil); 

if Long > pi 

Long = Long-2*pi; 

end 

if Long < -pi 

Long = Long+2*pi; 

end 
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Environmental Effects- Simulink Block 
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EFI 2 ECEF-Embedded Matlab Code 


function [C_EN,C_NE]= ECI_2_ECEF(EarthRotation,time) 
% transformation of eci to ecef coordinates 

theta = EarthRotation(3)*time; 

C_EN = [ cos(theta) -sin(theta) 0; 

sin(theta) cos(theta) 0; 

0 0 1 ] ; 

C_NE =C_EN'; 


Magnetic Dipole Model - Embedded Matlab Code 

function B_eci = Earth_Mag_Field(R) 

% Magnetic dipole model - in Tesla 

theta = 11.7; 

DCM = [1 0 0; 0 cosd(theta) sind(theta); 
muO = 4*pi*10e-7; 

M = DCM*[0 0 8e22]'; 

r = norm(R); 
r_hat = R/r; 

B_eci = muO*(3*dot(M,r_hat)*r_hat-M)/(4*pi*r*3); 


0 -sind(theta) 


% deg 

cosd(theta)]' 
% N/Amp ^ 2 
% A*m''2 


S hatl - Embedded Matlab Code 


function [S_body,S_inertial] = S(C_BO,C_NB,beta,TA) 

% This block supports an embeddable subset of the MATLAB language. 
% See the help menu for details. 

B=beta; 

S_orbit = [cos(B)*sin(TA); ... 

sin (B); ... 
cos(B)*cos(TA)]; 

S_body = C_BO*S_orbit; 

S_inertial = C_NB*S_body; 
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Dynamics and Kinematics - Simulink Block 
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omega of 
orbit in inertiai 


w_ON 


88 








torque2omegadot.m EML 


function Wdot = torque2omegadot(T, J, W) 

% This function takes input of applied torque (T) in component 
% elements, current angular velocity (W) in component 
% elements, and the moment-of-inertia matrix (J) as a diagonal 
% matrix containing the MOIs for the principal axes of the body 
% along the diagonal. Angular acceleration is then computed and 
% output as a 3x1 vector (Wdot). 

Wx = W(l); Wy = W(2); Wz = W(3); 

Jxx = J(l,l); Jyy = J(2,2); Jzz = J(3,3); 

Tx = T(l); Ty = T(2); Tz = T(3); 

wdotx = (Tx-(Jzz-Jyy)*Wz*Wy)/Jxx; 
wdoty = (Ty-(Jxx-Jzz)*Wx*Wz)/Jyy; 
wdotz = (Tz-(Jyy-Jxx)*Wy*Wx)/Jzz; 

Wdot = [wdotx; wdoty; wdotz]; 
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Disturbance Torques- Simulink Block 



Solar 
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Torque Gravity Gradient - EML 


function Tgg = T_Grav_Grad(C_BO, r, J, mu) 

% T_Grav_Grad takes inputs of the spacecraft inertia matrix (J), 

% current orbit radius (r) in m, and the Orbit-to-Body Frame DCM (C_BO) 
% to calculate the gravity gradient torque in the body frame (Tgg) and 
% orbit frame (T_o). The orb_vec vector defines which orbit frame axis 
% is aligned with the force producing the torque. In this case, the z- 
axis 

% points along nadir in the orbit frame, and corresponds to the r- 
vector 
% direction. 

orb_vec = [0; 0; 1]; 

c = C_BO*orb_vec; 

Jxx = J (1, 1) ; 

Jyy = J(2,2); 

Jzz = J (3, 3) ; 

Tgg = 3*mu/r''3* [ (Jzz-Jyy) *c (2) *c (3) ; . . . 


(Jxx-Jzz) *c(l)*c(3);... 
(Jyy-Jxx)*c(l)*c(2)] ; 


Torque Aerodynamic - Simulink Block 
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Solar Torque - Simulink Block 



Calculate Solar Torque - Simulink Block 


s_body 



Sum of 
Elements 














Sensors - Simulink Block 


1 

w BN 


Om ega_g 


-► Wjrue 


bias 


Gyro 


-XUD 

w_BNm 

>CZD 

bias 


i 3 > 

q_BN 


<q_BN] 


EulerBN 321 


[q_BNj 



Star Tracker 


q_BNm 



[q_BN ]:>-^ clBN 

B 

6 -►b 

^ Mag Inertial 

to Body 


B B sense 

Magneto - 
meter Model 


Bm 




94 







































Star Tracker - Embedded Matlab Code 


function q = StarTracker(u,qbn) 

ph=u(l)/2; th=u(2)/2; ps=u(3)/2; 

sph = sin(ph); sth = sin(th); sps = sin(ps); 
cph = cos(ph); cth = cos(th); cps = cos(ps); 

q = [sph*cth*cps-cph*sth*sps; 
cph*sth*cps+sph*cth*sps; 
cph*cth*sps-sph*sth*cps; 
cph*cth*cps + sph*sth*sps] ; 

[Y I]=max(abs(q)); 

q=q/norm(q)*sign(q(I, 1)/qbn(1,1)) ; 


Sun Sensors - Block Diagram 

DCM for 
Sun Sensor 1 



ATT.m 

function att = ATT( quat ) 

att = transpose(XT(quat)) * PSI(quat); 

return 
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Sun Sensor Facing YO - Simulink Block 



Sun Sensor Facing YOl - Block Diagram 

Ss 
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Mag Inertial to Body - Simulink Block 
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Multiplicative Quaternion Extended Kalman Filter- Simulink Block 
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MEKF - Embedded Matlab Code 


function [wkl,qkl,biaskl,Pkl] = EKF(wklt,q_init,bias_init,Bkl,B,dt,sig) 

sig_v = sig(1); 
sig_u = sig (2); 
sig_mag = sig(5); 

persistent qk biask wk Pk; 

% Initialize States and Measurement 
if isempty(qk) 
qk=q_init; 
biask = bias_init; 
wk = wklt; 

Pk= [ (0.8)''2*eye (3) zeros(3); zeros(3) (3*pi/180)''2*eye (3) ] ; 

wkl=wk; 
qkl=qk; 
biaskl=biask; 

Pkl=Pk; 
return ; 

end 

%% Propagation 
biaskl = biask; 


Skew_w = SKEW(wk); 

Mag_w = norm(wk); 

psik = (sin(l/2*Mag_w*dt)/Mag_w)*wk; 

Omega = [cos(l/2*Mag_w*dt)*eye(3)-SKEW(psik) psik; 

-psik' cos(l/2*Mag_w*dt) ]; 

qkl = Omega*qk; 

Phi_ll = eye (3)-Skew_w*sin (Mag_w*dt)/Mag_w + Skew_w''2*(l- 
cos(Mag_w*dt))/Mag_w^2; % 7.59b 

Phi_12 = Skew_w* (1-cos (Mag_w*dt))/Mag_w''2 - eye(3)*dt -... 

% 7.59c 

Skew_w^2*(Mag_w*dt-sin(Mag_w*dt))/Mag_w^3; 

Phi_21 = zeros(3); 

% 7.59d 

Phi_22 = eye(3); 

% 7.59e 

Phi = [Phi_ll Phi_12; Phi_21 Phi_22]; 

% 7.59a 

Gk = [-eye(3) zeros(3); zeros(3) eye(3)]; 

Qk = [ (sig_v'2*dt + l/3*sig_u''2*dt''3) *eye (3) - (l/2*sig_u''2*dt^2) *eye (3) 
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(l/2*sig_u^2*dt^2)*eye(3) 


(sig_u^2*dt)*eye(3) 


] ; 

Pkl = Phi*Pk*Phi'+Gk*Qk*Gk' ; 

%% Update Loop - 

Att = ATT(qkl); 
delX = zeros (6,1); 

% Update for Magnetometer Measurement - 

H = [SKEW(Att*B) zeros (3,3)]; 

R = sig_mag^2*eye (3); 

% Gain 

K = (Pkl*H')/(H*Pkl*H' + R); 

% Update 

Pkl = (eye(6) - K*H)*Pkl; 

res = Bkl - Att*B; 

delX = delX + K*(res-H*delX); 

qkl = qkl+l/2*XI(qkl)*delX(1:3,:); 
qkl = qnormalize(qkl'*qkl, qkl) ; 

biaskl = biaskl + delX(4:6,:); 

wkl = wklt - biaskl; 

% Save previous values 
qk = qkl; 
biask = biaskl; 
wk = wkl; 

Pk = Pkl; 

return 

"6 

%% Normalizing routine for quaternions 
function qkl = qnormalize(qnorm,qkl) 
while (qnorm) > 1 

if qnorm < 1 + le-9 

qkl = ((3 + qnorm)/(I + 3*qnorm))*qkl; 
% rescale quaternion to (err^3)/32 

else 

qkl = qkl/sqrt(qnorm); 

% renormalize quaternion 

end 

qnorm = qkl'*qkl; 

end 

return 

"6 
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Unscented Kalman Filter - USQUE- Simulink Block 



w BNf u 


q_BNf_u 


bias f u 


► 


MATLAB 

Function 


diag 



Pdiag _u 


MATLAB 

Function 






Pnorm u 


norm 
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UKF- USQUE- Embedded Matlab Code 


function [wkl,qkl,biaskl,Pxx_kl] = 

UKF(wklt,q_init,bias_init,Bkl,B,dt,sig,lambda,a) 


Initialization 


% Variance of Sensors 
sig_v = sig(1) ; 

sig_u = sig(2); 

sig_mag = sig(5); 

f = 2* (a+1); % Ref [3] pg 6 


persistent qk biask Pxx_k 
% Initialize States and Measurement 
if isempty(qk) 
qk=q_init; 
biask = bias_init; 
disp(q_init); 
disp(bias_init); 

Pxx_k= [ (1)''2*eye (3) zeros(3); zeros(3) (3*pi/180)''2*eye (3) ] ; 

end 


"6 

"6 


Calculation of Sigma Points 


Qbar_k = dt/2* [ (sig_v''2-l/6*sig_u''2*dt''2) *eye (3) zeros (3) ; 

% Ref [3] 42 

zeros (3) sig_u''2*eye (3) ]; 


% Sigma points equations 
x_k = [[0 0 0]'; biask]; 

Dx=size(x_k,1); 

Dy=size(Bkl,1); 

NSig=2*Dx+l; 

sig_x=chol((Dx+lambda)*(Pxx_k+Qbar_k))'; % Ref 

[3] 5a 
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Ref [3] 


chi_sig_k=x_k*ones(1,NSig)+[zeros(Dx,1) sig_x -sig_x]; 

5a 

chi_sig_kl=zeros(6,NSig); % Ref [3] 32 

y_sig_kl=zeros(3,NSig) ; 
q_kl=zeros (4,1) ; 


for i=l:NSig 


del_q_k=delp2delq(chi_sig_k(1:3,i),a,f); 
q_sig_k = quaterr(del_q_k, qk); 


Propagate Forward the Quaternion (still in the loop!) 


w_sig_k = wklt - chi_sig_k(4:6,i); 

Mag_w = norm(w_sig_k); 

psik = (sin(l/2*Mag_w*dt)/Mag_w)*w_sig_k; 
zk = cos(l/2*Mag_w*dt)*eye(3)-SKEW(psik); 

Omega = [ zk, psik; 

-psik', cos(l/2*Mag_w*dt) ]; 

q_sig_kl = Omega*q_sig_k; 

"0 = = = = = = = = = = = = = = = = = = = = = = = = = 

% Saving the q(-)k+l(0) 

-g = = = = = = = = = = = = = = = = = = = = = = = = = 

if i==l 

q_k1=q_sig_k1; 

end 


% Ref [3] 35 


% Ref [3] 29 

% Ref [3] 34 

% Ref [3] 36 


del_q_kl = quaterr(q_sig_kl, [-q_kl(1:3,1);q_kl(4,1)]); 
chi_sig_kl(1:3,i) = f*del_q_kl(1:3,1)/(a+del_q_kl(4,1)); % Ref [3] 

37b 

chi_sig_kl(4:6,i) = chi_sig_k(4:6,i); 
y_sig_kl(:,i) = ATT(q_sig_kl)*B; 

0 , 

0 


% Note: The bias does not change so chi_sig_kl(4:6,i) stays the 

same 

"5 


end 
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Following USQUE Method Ref [3] pg 6 


% Calculating Weights 
R = sig_mag^2*eye(3) ; 

W=ones(NSig,1)/(2*(Dx+lambda)); 
W(1,1)=lambda/(Dx+lambda); 

% Mean Point Calculations 

x_klp=chi_sig_kl*W; 

y_klp=y_sig_kl*W; 


Covariance and Gain Calculations 


% Error Covariance Calculation 
Pxx_klp=Qbar_k; 

Pyy_klp=R; 

Pxy_klp=zeros(Dx, Dy) ; 
for 1=1:NSig 

xdif=chi_sig_kl(:,1)-x_klp; 
ydif=y_sig_kl(:,1)-y_klp; 

Pxx_klp=Pxx_klp+xdif*xdif'*W(1, 1) ; 
Pyy_klp=Pyy_klp+ydif*ydif'*W(1, 1); 
Pxy_klp=Pxy_klp+xdif*ydif'*W(1, 1); 

end 

% Gain and Update 
K = Pxy_klp/Pyy_klp; % Gain 

Pxx_kl = Pxx_klp-K*Pxy_klp'; % Error Covariance Update 

x_kl = x_klp+K*(Bkl-y_klp); % State Update 

% Calculation of Updated Quaternion! 
del_q_kl=delp2delq(x_kl(1:3, :) , a, f) ; 
qkl = quaterr(del_q_kl,q_kl); 
qkl = qnormalize(qkl) ; 
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biaskl = x_kl(4:6,l); 
wkl = wklt-biaskl; 


Setup for Next Update 


qk=qkl; 
biask=biaskl; 
Pxx_k = Pxx_kl; 

return 

"6 

"6 

"6 "0 


%% Normalizing routine for quaternions 
function qkl = qnormalize(qkl) 
qnorm=qkl'*qkl; 
while (qnorm) > 1 

if qnorm < 1 + le-9 

qkl = ((3 + qnorm)/(I + 3*qnorm))*qkl; 
% rescale quaternion to (err^3)/32 

else 

qkl = qkl/sqrt(qnorm); 

% renormalize quaternion 

end 

qnorm = qkl'*qkl; 

end 

return 

"6 
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o\° o\° o\° 


ADS MainScriptm 


%% Spacecraft Attitude Determination Script 
% Note that this code runs both the EKF and the UKF 


Created by Orlando X. Diaz 
Advisor Dr. Marcelo Romano 
Co-Advisor Dr. Hyun-wook Woo 

%% Format 

clear all 
close all 
clc 

global CONST 
R2D = 180/pi; 

D2R = pi/180; 

%% Set Simulation Conditions 


InitialFuler = [0,0,0];%deg 
ReferenceFuler =[00 0];%deg 


*** Toggle switches turn the labeled functions on (1) or off (0). 


Tgg_toggle 

= 

1; 

Taero_toggle 


1; 

Tsolar_toggle 


1; 

timeOn 


1; 

taOn 


0; 

cboOn 


0; 

qbnOn 


1; 

qbnmOn 


1; 

rOn 


0; 

hOn 


0; 

e3210n 


1; 

wbnOn 


1; 

tcOn 


0; 

hsOn 


0; 

wbnfOn 


1; 

biasOn 


1; 

biasfOn 


1; 

pdOn 


1; 

pnOn 


1; 

qbnfOn 


1; 

wbnmOn 


1; 

werrOn 


1; 

berrOn 


1; 

qerrOn 


1; 
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%% Set Constants 
CONST.mu 

CONST.mu_moon = 

CONST.mu_sun = 

CONST.Re 

CONST.Rs 

CONST.J2 

CONST.J3 

CONST.J4 

CONST.SolarPress= 
CONST.SOLARSEC = 
CONST.w_earth 
CONST.Cd 
CONST.Cr 
Reflect 

CONST.OmegaDot = 
advance for sun-synch 


398.6004418el2 ; %m''3/s''2 
4.902802953597el2; %m''3/s''2 
1.32 7122E2 0; %m^3/s^2 
6.378137E6; %m 
1.4959787ell; %m 
1.08262668355E-3; % 

-2.53265648533E-6; % 

-1.61962159137E-6; % 

4.51e-6; %N/m^2 
806.81112382429; %TU 
-[0;0; .000072 9211585530];% 
2.5;% 

. 6 ; % 

1.991e-7; %rad/s 


earth radius 
solar radius 
J2 term 
J3 term 
J4 term 

solar wind pressure 

r/s earth rotation 

Coefficient of Drag 
Coefficient of 

ascending node 


%% Set Orbital Elements 

%Kep elements meters and radians (a,e,1,W,w,n) 

altitude at perigee 
altitude at apogee 


h_p = 500e3;%m 

h_a = 500e3;%m 


RAAN = 0;%rad 

w = 0;%rad 

TAo = 0;%rad 

Rp = CONST.Re+h_p; %m 

Ra = CONST.Re+h_a; %m 

e = (Ra-Rp) / (Ra+Rp) ; % (m/m) 

a = (Ra+Rp)/2;%m 

ho = sqrt(a*CONST.mu*(l-e^2)); %my2/s 
momentum 


Right Ascention 
argument of perigee 
true anomaly 
radius of perigee 
radius of apogee 
eccentricity 
semi-major axis 
initial angular 


P = 2*pi*(a^S/CONST.mu)*.5; %sec Orbit Period 

i_sunsynch = acosd((CONST.OmegaDot*(l-e^2)^2*a^(7/2 )).. . 

/ (-3/2*sqrt (CONST.mu) *CONST.J2*CONST.Re''2) ) ;%eqn 4.47 from 

Curtis 

i = i_sunsynch*D2R; %deg (rad) orbit inclination 


[Ro,Vo] = sv_from_coe(CONST.mu,[ho e RAAN i w TAo]);% 
orbital state vector 


initial 


%% Set ICs 

w_BNo = [0;2*pi/P;0]; %rad initial body rates 
w_ON = [0;2*pi/P;0]; %rad 

rand( 'seed' ,2); 
randn( 'seed' , 2) ; 
seedarw=l; 
seedrrw=2; 
seedst=3; 
seedmag=4; 
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% Sensor parameters 
% Gyro 

GYRO_Bias = (3*randn(3,1))*pi/180; % + 3 deg/sec 

N_ARW = (0.029)*pi/180; 

K_RRW = (0.0002)*pi/180; 

ARW = N_ARW^2; % angular white noise Variance 

RRW = K_RRW''2/3; % bias variance 

Gg = eye(3) .* (-0.01 + 0.02*rand(3)) +... 

(ones(3,3)-eye(3)).*(-0.0006+0.0012*rand(3)); %percent 

% Magnetometer 
sigMag = 1.25e-7; 

Gm = eye(3).*(-0.02+0.04*rand(3)) +... 

(ones(3,3)-eye (3)) .*(-0.0028 + 0.0056*rand(3)); %percent 

% Sun Sensor 

51 = [0 45 0]'*pi/180; 

52 = [45 0 0]'*pi/180; 

SS_nl = [1 0 0]; 

SS_n2 = [1 0 0] ; 

FOV = 0.7; 
sigSS = 0.1; 

J = Bessel(sigSS/2,FOV).*pi/180; 

% Star Tracker 

sigST = 70 /3 /60 /60*pi/180; %arcsec to rad (3sig) 

% Kalman Filter 
dt = 0.05; 
t_ekf = dt; 
sig(l) = sqrt(ARW); 
sig(2) = sqrt(RRW); 
sig(3) = sigST; 
sig(4) = sigSS*pi/180; 
sig(5) = sigMag; 

ReferenceOmega = w_ON; 

[qBOo] = Euler_to_Quaternion(InitialEuler) ; 

[ReferenceQuaternion] = Euler_to_Quaternion(ReferenceEuler); 

qBNo = qBOo; 

%% Run Simulation 
[Spacecraft]= SCproperties; 

J_Matrix = Spacecraft.MOI; 

[density_table] = GetDensity; 

RunTime = 4000; %sec 


%sec (20 Hz) model speed 
%sec (100 Hz) ekf speed 
%rad/Hz''(1/2) , ARW 
%rad/sec^(3/2), RRW 
%rad. Star Tracker Error 
%rad. Sun Sensor Error 
%tesla, magnetometer error 
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tic 

sim( ' ADS_SpacecraftSim ' ,RunTime); 

Total_Model_time = toe 

factor = RunTime/Total_Model_time 


DisturbanceTorque 

DisturbanceTorque 

DisturbanceTorque 

SensorMeasurement 

SensorMeasurement 

SensorMeasurement 

SensorMeasurement 

SensorMeasurement 

SensorMeasurement 


Tgg = 

Tgg; 

Taero 

= Taero; 

Tsolar 

= Tsolar 

ST = q 

_BNm; 

Gyro = 

w_BNm; 

bias = 

bias; 

SSI = 

ssl; 

SS2 = 

ss2; 

Mag = 

Bm; 


FilterEst 

FilterEst 

FilterEst 


Q = q_BNf; 

Gyro = w_BNf; 
bias = bias_f; 


PlotUKFError.m 


bias_e_ul=squeeze(bias_e_ul); 

[m,n]=size(bias_e_ul); 
if m<n 

bias_e_u1=bias_e_u 1'; 

end 

for i=l:3 

figure (1) 
subplot(3,1,i) 

plot(SimTimel,p_BNe_ul(:,!)) 
hold on 

plot(SimTimel,3*sqrt(Pdiag_ul(:,!)), '-.r') 

plot(SimTimel,-3*sqrt(Pdiag_ul(:,i)), '-.r') 

grid on 

xlim([0 4000]) 

ylim([-.2 .2]) 

xlabel('Time (Sec) ') 

label=[ '\deltap' num2str(i)]; 

ylabel (label) 

end 

subplot(3,1,2) 
ylim([-.05 .05]) 
subplot(3,1,1) 

title (' Generalized Rodriquez Parameter Error for UKF' ) 

for i=4:6 

figure (2) 
subplot(3,1,i-3) 

plot(SimTimel,bias_e_ul(:,i-3)); 
hold on 

plot(SimTimel,3*sqrt(Pdiag_ul(:,i)), '-.r'); 

plot(SimTimel,-3*sqrt(Pdiag_ul(:,!)), '-.r'); 

grid on 

xlim([0 4000]) 

ylim([-5E-4 5E-4]) 

xlabel('Time (Sec) ') 

label=[ '\delta \beta ' num2str(i-3)]; 
ylabel(label) 

end 

subplot(3,1,1) 

title ('Bias Error for UKF') 


bias_e_el=squeeze(bias_e_el); 
[m,n]=size(bias_e_el); 
if m<n 
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bias_e_e1=bias_e_e1'; 

end 

for i=l:3 

figure (3) 
subplot(3,1,i) 

plot(SimTimel,q_BNe_el(:,!)) 
hold on 

plot(SimTimel, 3/2*sqrt(Pdiagl(: , 1)) , '-.r' ); 

plot(SimTimel,-3/2*sqrt(Pdiagl(:, 1)) , '-.r' ); 

grid on 

xlim([0 4000]) 

ylim([-.15 .15]) 

ylim([-.2 .2]) 

xlabel('Time (Sec) ') 

label=[ '\deltaq' num2str(i)]; 

ylabel(label) 

end 

subplot(3,1,1) 

title (' Quaternion Error for EKE') 
subplot(3,1,2) 
ylim([-.02 .02]) 

for 1=4:6 

figure (4) 
subplot(3,1,1-3) 
plot(SimTimel,bias_e_el(:,1-3)) 
hold on 

plot(SimTimel, 3*sqrt(Pdiagl(:, 1)) , '-.r' ); 

plot(SimTimel,-3*sqrt(Pdiagl(:, 1)) , '-.r' ); 

grid on 

xlim([0 4000]) 

ylim([-5E-4 5E-4]) 

xlabel('Time (Sec) ') 

label=[ '\delta \beta ' num2str(i-3)]; 
ylabel(label) 


end 

subplot(3,1,1) 

title ('Bias Error for EKE') 

clear norm_p_u norm_p_e norm_bias_u norm_bias_e 

norm_p_u=zeros(m, 1); 

norm_p_e=zeros(m, 1) ; 

norm_bias_u=zeros(m, 1) ; 

norm_bias_e=zeros(m, 1); 

for i=l:m; 

norm_p_u(i,1)=norm(p_BNe_ul(i,:)); 
norm_p_e(i,1)=norm(2*p_BNe_el(i,:)); 
norm_bias_u(i,1)=norm(bias_e_ul(i, :)); 
norm_bias_e(i,1)=norm(bias_e_el(i,:)); 
end 

figure(5) 
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semilogy(SimTimel,norm_p_u) 
hold on 

semilogy(SimTimel,norm_p_e, '-.r' ) 
grid on 

title (' Normalized EKF and UKF Attitude Errors') 
xlabel('Time (Sec) ') 
ylabel (' Attitude Errors') 

legend (' Normalized UKF Generalized Rodriguez Parameter Errors', 
'Normalized EKF Generalized Rodriguez Parameter Errors') 
xlim([0 4000]) 

figure(6) 

semilogy(SimTimel,norm_bias_u) 
hold on 

semilogy(SimTimel,norm_bias_e, '-.r' ) 
grid on 

title (' Normalized EKF and UKF Bias Errors') 

xlabel('Time (Sec) ') 

ylabel (' Normalized \beta Errors') 

legend (' Normalized UKF Bias Errors', 'Normalized EKF Bias Errors') 
ylim([0 .01]) 
xlim([0 4000]) 
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